IMU orientation Filter developed by Sebastian Madgwick(IMU数据融合)

一篇写的非常好的解析:

Madgwick IMU Filter

IMU Data Fusing: Complementary, Kalman, and Mahony Filter
关于Madgwick算法的介绍:

Madgwick算法（上）

Madgwick算法（下）

欧拉角、四元数和旋转矩阵

macrodefs.h代码(主要是宏定义):

#ifndef MACRODEFS_H
#define MACRODEFS_H

// CAN Frame Micro Defines
#define GYRO_X_Y 0x00000066
#define GYRO_Z_Temp 0x00000067
#define ACCEL_X_Y_Z 0x00000069

#define GYRO_FULL_32_BIT	((unsigned int)(0xFFFFFFFF))
#define ACCEL_FULL_16_BIT   ((unsigned int)(0xFFFF))

#define GYRO_RATE_LSB 0.001
#define ACCEL_RATE_LSB 0.0001
#define TEMP_IMU_LSB 1

/* Blow macros are imported from http://mbed.org/cookbook/IMU and may not be used
 */

//Gravity at Earth's surface in m/s/s
#define g0 9.812865328

//Number of samples to average.
#define SAMPLES 4

//Number of samples to be averaged for a null bias calculation
//during calibration.
#define CALIBRATION_SAMPLES 128

//Convert from radians to degrees.
#define toDegrees(x) (x * 57.2957795)

//Convert from degrees to radians.
#define toRadians(x) (x * 0.01745329252)

//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
#define GYROSCOPE_GAIN (1 / 14.375)

//Full scale resolution on the ADXL345 is 4mg/LSB.
#define ACCELEROMETER_GAIN (0.004 * g0)

//Sampling gyroscope at 200Hz.
#define GYRO_RATE   0.005

//Sampling accelerometer at 200Hz.
#define ACC_RATE    0.005

//Updating filter at 1/FILTER_RATE Hz.
#define FILTER_RATE 0.01

// Timer Interval, namely 1000*FILTER_RATE
#define TIMER_INTERVAL 10

// Sample Frames for Calibration
#define SAMPLE_FRAMES_FOR_CALIBRATION 10000

#endif // MACRODEFS_H

IMUfilter.h代码:

/**
 * @author Aaron Berk
 *
 * @section LICENSE
 *
 * Copyright (c) 2010 ARM Limited
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @section DESCRIPTION
 *
 * IMU orientation filter developed by Sebastian Madgwick.
 *
 * Find more details about his paper here:
 *
 * http://code.google.com/p/imumargalgorithm30042010sohm/
 */

#ifndef IMU_FILTER_H
#define IMU_FILTER_H

/**
 * Includes
 */

#include 
#include "macrodefs.h"

/**
 * Defines
 */
#define PI 3.1415926536

/**
 * IMU orientation filter.
 */


class IMUfilter {

public:

    /**
     * Constructor.
     *
     * Initializes filter variables.
     *
     * @param rate The rate at which the filter should be updated.
     * @param gyroscopeMeasurementError The error of the gyroscope in degrees
     *  per second. This used to calculate a tuning constant for the filter.
     *  Try changing this value if there are jittery readings, or they change
     *  too much or too fast when rotating the IMU.陀螺仪误差
     */
    IMUfilter(double rate, double gyroscopeMeasurementError);

    /**
     * Update the filter variables.
     *
     * @param w_x X-axis gyroscope reading in rad/s.
     * @param w_y Y-axis gyroscope reading in rad/s.
     * @param w_z Z-axis gyroscope reading in rad/s.
     * @param a_x X-axis accelerometer reading in m/s/s.
     * @param a_y Y-axis accelerometer reading in m/s/s.
     * @param a_z Z-axis accelerometer reading in m/s/s.
     */
    void updateFilter(double w_x, double w_y, double w_z,
                      double a_x, double a_y, double a_z);

    /**
     * Compute the Euler angles based on the current filter data.
     */
    void computeEuler(void);

    /**
     * Get the current roll.
     *
     * @return The current roll angle in radians.
     */
    double getRoll(void);

    /**
     * Get the current roll.
     *
     * @return The current roll angle in degrees.
     */
    double getRollInDegrees(void);

    /**
     * Get the current pitch.
     *
     * @return The current pitch angle in radians.
     */
    double getPitch(void);

    /**
     * Get the current pitch.
     *
     * @return The current pitch angle in degrees.
     */
    double getPitchInDegrees(void);

    /**
     * Get the current yaw.
     *
     * @return The current yaw angle in radians.
     */
    double getYaw(void);

    /**
     * Get the current yaw.
     *
     * @return The current yaw angle in degrees.
     */
    double getYawInDegrees(void);

    /**
     * Reset the filter.
     */
    void reset(void);

private:

    int firstUpdate;

    //Quaternion orientation of earth frame relative to auxiliary frame.
    double AEq_1;
    double AEq_2;
    double AEq_3;
    double AEq_4;

    //Estimated orientation quaternion elements with initial conditions.
    double SEq_1;
    double SEq_2;
    double SEq_3;
    double SEq_4;

    //Sampling period
    double deltat;

    //Gyroscope measurement error (in degrees per second).
    double gyroMeasError;

    //Compute beta (filter tuning constant..
    double beta;

    double phi;
    double theta;
    double psi;

};

#endif /* IMU_FILTER_H */

IMUfilter.cpp代码:

/**
 * @author Aaron Berk
 * 
 * @section LICENSE
 *
 * Copyright (c) 2010 ARM Limited
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 *
 * @section DESCRIPTION
 *
 * IMU orientation filter developed by Sebastian Madgwick.
 *
 * Find more details about his paper here:
 *
 * http://code.google.com/p/imumargalgorithm30042010sohm/ 
 */

/**
 * Includes
 */
#include "IMUfilter.h"

IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){

    firstUpdate = 0;
    
    //Quaternion orientation of earth frame relative to auxiliary frame.
    AEq_1 = 1;
    AEq_2 = 0;
    AEq_3 = 0;
    AEq_4 = 0;
    
    //Estimated orientation quaternion elements with initial conditions.
    SEq_1 = 1;
    SEq_2 = 0;
    SEq_3 = 0;
    SEq_4 = 0;

    //Sampling period (typical value is ~0.1s).
    deltat = rate;
    
    //Gyroscope measurement error (in degrees per second).
    gyroMeasError = gyroscopeMeasurementError;
    
    //Compute beta.
    beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0));

}

void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) {

    //Local system variables.

    //Vector norm.
    double norm;
    //Quaternion rate from gyroscope elements.
    double SEqDot_omega_1;
    double SEqDot_omega_2;
    double SEqDot_omega_3;
    double SEqDot_omega_4;
    //Objective function elements.
    double f_1;
    double f_2;
    double f_3;
    //Objective function Jacobian elements.
    double J_11or24;
    double J_12or23;
    double J_13or22;
    double J_14or21;
    double J_32;
    double J_33;
    //Objective function gradient elements.
    double nablaf_1;
    double nablaf_2;
    double nablaf_3;
    double nablaf_4;

    //Auxiliary variables to avoid reapeated calcualtions.
    double halfSEq_1 = 0.5 * SEq_1;
    double halfSEq_2 = 0.5 * SEq_2;
    double halfSEq_3 = 0.5 * SEq_3;
    double halfSEq_4 = 0.5 * SEq_4;
    double twoSEq_1 = 2.0 * SEq_1;
    double twoSEq_2 = 2.0 * SEq_2;
    double twoSEq_3 = 2.0 * SEq_3;

    //Compute the quaternion rate measured by gyroscopes.
    SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
    SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
    SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
    SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;

    //Normalise the accelerometer measurement.
    norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
    a_x /= norm;
    a_y /= norm;
    a_z /= norm;

    //Compute the objective function and Jacobian.
    f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
    f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
    f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
    //J_11 negated in matrix multiplication.
    J_11or24 = twoSEq_3;
    J_12or23 = 2 * SEq_4;
    //J_12 negated in matrix multiplication
    J_13or22 = twoSEq_1;
    J_14or21 = twoSEq_2;
    //Negated in matrix multiplication.
    J_32 = 2 * J_14or21;
    //Negated in matrix multiplication.
    J_33 = 2 * J_11or24;

    //Compute the gradient (matrix multiplication).
    nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
    nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
    nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
    nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;

    //Normalise the gradient.
    norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
    nablaf_1 /= norm;
    nablaf_2 /= norm;
    nablaf_3 /= norm;
    nablaf_4 /= norm;

    //Compute then integrate the estimated quaternion rate.
    SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
    SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
    SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
    SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;

    //Normalise quaternion
    norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
    SEq_1 /= norm;
    SEq_2 /= norm;
    SEq_3 /= norm;
    SEq_4 /= norm;

    if (firstUpdate == 0) {
        //Store orientation of auxiliary frame.
        AEq_1 = SEq_1;
        AEq_2 = SEq_2;
        AEq_3 = SEq_3;
        AEq_4 = SEq_4;
        firstUpdate = 1;
    }

}

void IMUfilter::computeEuler(void){

    //Quaternion describing orientation of sensor relative to earth.
    double ESq_1, ESq_2, ESq_3, ESq_4;
    //Quaternion describing orientation of sensor relative to auxiliary frame.
    double ASq_1, ASq_2, ASq_3, ASq_4;    
                              
    //Compute the quaternion conjugate.
    ESq_1 = SEq_1;
    ESq_2 = -SEq_2;
    ESq_3 = -SEq_3;
    ESq_4 = -SEq_4;

    //Compute the quaternion product.
    ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
    ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
    ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
    ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;

    //Compute the Euler angles from the quaternion.
    phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
    theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3);
    psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);

}

double IMUfilter::getRoll(void){

    return phi;

}

double IMUfilter::getPitch(void){

    return theta;

}

double IMUfilter::getYaw(void){

    return psi;

}

// Degrees
double IMUfilter::getRollInDegrees(void){

    return toDegrees(phi);

}

double IMUfilter::getPitchInDegrees(void){

    return toDegrees(theta);

}

double IMUfilter::getYawInDegrees(void){

    return toDegrees(psi);

}

void IMUfilter::reset(void) {

    firstUpdate = 0;

    //Quaternion orientation of earth frame relative to auxiliary frame.
    AEq_1 = 1;
    AEq_2 = 0;
    AEq_3 = 0;
    AEq_4 = 0;

    //Estimated orientation quaternion elements with initial conditions.
    SEq_1 = 1;
    SEq_2 = 0;
    SEq_3 = 0;
    SEq_4 = 0;

}