imu ahrs.rar_9轴 姿态_IMU 姿态_IMU AHRS_imu C语言_融合

//===================================================================================================== // AHRS.c // S.O.H. Madgwick // 25th August 2010 //=================================================================================================// Description: // // 四元的“DCM的过滤器”的实施[Mayhony等]。采用了磁性失真 // 我的过滤器[Madgwick]省去了补偿算法 引用 // 通量（BX BZ）的方向，是预定义的和限制的磁场效应 扭曲到偏航 // axis only. // // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. // // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated // 方向. 见我概述使用这个应用程序中的四元报告。 // // 用户必须调用 'AHRSupdate()' 校准每个采样周期和解析 gyroscope ('gx', 'gy', 'gz'), // 加速度计 ('ax', 'ay', 'ay') and 磁力仪('mx', 'my', 'mz') data. Gyroscope units are // 弧度/秒，加速度计和磁强计单位归为载体无关，是归为载体。 // //===================================================================================================== //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include <math.h> //---------------------------------------------------------------------------------------------------- // Definitions #define Kp 2.0f // 比例增益支配收敛率accelerometer/magnetometer #define Ki 0.005f // 积分增益执政速率陀螺仪的衔接gyroscopeases #define halfT 0.5f // 采样周期的一半 //--------------------------------------------------------------------------------------------------- // Variable definitions float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计 方向 float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 //==================================================================================================== // Function //==================================================================================================== void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float norm; float hx, hy, hz, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; // 辅助变量，以减少重复操作数 float q0q0 = q0*q0; float q0q1 = q0*q1; float q0q2 = q0*q2; float q0q3 = q0*q3; float q1q1 = q1*q1; float q1q2 = q1*q2; float q1q3 = q1*q3; float q2q2 = q2*q2; float q2q3 = q2*q3; float q3q3 = q3*q3; // 测量正常化 norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; norm = sqrt(mx*mx + my*my + mz*mz); mx = mx / norm; my = my / norm; mz = mz / norm; // 计算参考磁通方向 hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2); hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1); hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2); bx = sqrt((hx*hx) + (hy*hy)); bz = hz; //估计方向的重力和磁通（V和W） vx = 2*(q1q3 - q0q2); vy = 2*(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3; wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2); wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3); wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2); // 错误是跨产品的总和之间的参考方向的领域和方向测量传感器 ex = (ay*vz - az*vy) + (my*wz - mz*wy); ey = (az*vx - ax*vz) + (mz*wx - mx*wz); ez = (ax*vy - ay*vx) + (mx*wy - my*wx); // 积分误差比例积分增益 exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // 调整后的陀螺仪测量 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; // 整合四元数率和正常化 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; // 正常化四元 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; } //==================================================================================================== // END OF CODE //====================================================================================================