Madgwick 和 Mahony算法源码

经典的四元数解算方法官方源码Madgwick 和 Mahony，里面有模拟示例数据和c和matlab类型的数据，特别适合开发所需要的姿态解算和惯性导航所需要的姿态解算。本人亲测代码可以使用。

//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick‘s IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date			Author          Notes
// 29/09/2011	SOH Madgwick    Initial release
// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
//
//=====================================================================================================

//---------------------------------------------------------------------------------------------------
// Header files

#include “MadgwickAHRS.h“
#include 

//---------------------------------------------------------------------------------------------------
// Definitions

#define sampleFreq	512.0f		// sample frequency in Hz
#define betaDef		0.1f		// 2 * proportional gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float beta = betaDef;								// 2 * proportional gain （Kp）
volatile float q0 = 1.0f q1 = 0.0f q2 = 0.0f q3 = 0.0f;	// quaternion of sensor frame relative to auxiliary frame

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt（float x）;

//====================================================================================================
// Functions

//---------------------------------------------------------------------------------------------------
// AHRS algorithm update

void MadgwickAHRSupdate（float gx float gy float gz float ax float ay float az float mx float my float mz） {
	float recipNorm;
	float s0 s1 s2 s3;
	float qDot1 qDot2 qDot3 qDot4;
	float hx hy;
	float _2q0mx _2q0my _2q0mz _2q1mx _2bx _2bz _4bx _4bz _2q0 _2q1 _2q2 _2q3 _2q0q2 _2q2q3 q0q0 q0q1 q0q2 q0q3 q1q1 q1q2 q1q3 q2q2 q2q3 q3q3;

	// Use IMU algorithm if magnetometer measurement invalid （avoids NaN in magnetometer normalisation）
	if（（mx == 0.0f） && （my == 0.0f） && （mz == 0.0f）） {
		MadgwickAHRSupdateIMU（gx gy gz ax ay az）;
		return;
	}

	// Rate of change of quaternion from gyroscope陀螺仪四元数的变化率
	qDot1 = 0.5f * （-q1 * gx - q2 * gy - q3 * gz）;
	qDot2 = 0.5f * （q0 * gx + q2 * gz - q3 * gy）;
	qDot3 = 0.5f * （q0 * gy - q1 * gz + q3 * gx）;
	qDot4 = 0.5f * （q0 * gz + q1 * gy - q2 * gx）;

	// Compute feedback only if accelerometer measurement valid （avoids NaN in accelerometer normalisation）
	if（!（（ax == 0.0f） && （ay == 0.0f） && （az == 0.0f））） {

		// Normalise accelerometer measurement
		recipNorm = invSqrt（ax * ax + ay * ay + az * az）;
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;   

		// Normali

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-12-10 17:35  Madgwick  Mahony\
     目录           0  2018-10-11 17:59  Madgwick  Mahony\madgwick_algorithm_c\
     目录           0  2018-10-11 20:06  Madgwick  Mahony\madgwick_algorithm_c\MadgwickAHRS\
     文件        9210  2018-12-10 17:23  Madgwick  Mahony\madgwick_algorithm_c\MadgwickAHRS\MadgwickAHRS.c
     文件        1519  2011-10-02 16:21  Madgwick  Mahony\madgwick_algorithm_c\MadgwickAHRS\MadgwickAHRS.h
     目录           0  2018-10-11 19:29  Madgwick  Mahony\madgwick_algorithm_c\MahonyAHRS\
     文件        8411  2018-12-10 17:23  Madgwick  Mahony\madgwick_algorithm_c\MahonyAHRS\MahonyAHRS.c
     文件        1573  2011-10-02 16:21  Madgwick  Mahony\madgwick_algorithm_c\MahonyAHRS\MahonyAHRS.h
     目录           0  2018-10-11 17:59  Madgwick  Mahony\madgwick_algorithm_matlab\
     目录           0  2018-10-11 17:59  Madgwick  Mahony\madgwick_algorithm_matlab\@MadgwickAHRS\
     文件        4678  2011-09-28 23:40  Madgwick  Mahony\madgwick_algorithm_matlab\@MadgwickAHRS\MadgwickAHRS.m
     目录           0  2018-10-11 17:59  Madgwick  Mahony\madgwick_algorithm_matlab\@MahonyAHRS\
     文件        4718  2011-09-28 23:40  Madgwick  Mahony\madgwick_algorithm_matlab\@MahonyAHRS\MahonyAHRS.m
     文件      101819  2011-09-24 13:39  Madgwick  Mahony\madgwick_algorithm_matlab\ExampleData.mat
     文件        3263  2012-11-06 19:00  Madgwick  Mahony\madgwick_algorithm_matlab\Examplescript.m
     目录           0  2018-10-11 17:59  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\
     文件        2465  2011-09-27 16:51  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\Testscript.m
     文件         650  2011-09-27 16:52  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\axisAngle2quatern.m
     文件         965  2011-09-27 16:52  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\axisAngle2rotMat.m
     文件         938  2011-09-27 16:52  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\euler2rotMat.m
     文件         878  2011-09-27 16:52  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\quatern2euler.m
     文件         825  2011-09-27 16:52  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\quatern2rotMat.m
     文件         378  2011-09-27 16:52  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\quaternConj.m
     文件         661  2011-09-27 16:51  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\quaternProd.m
     文件         645  2011-09-27 16:51  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\rotMat2euler.m
     文件        1434  2011-09-27 16:51  Madgwick  Mahony\madgwick_algorithm_matlab\quaternion_library\rotMat2quatern.m