http://www.docin.com/p-1244268274.html
#pragma once
#include <iostream>
#if 0
/*
float q0, q1, q2, q3;
float exInt, eyInt, ezInt;
float Kp, Ki;
float halfT;
float sampleFreq;
*/
/* Private define————————————————————*/
#define twoKp 2.0f // proportional gain governs rate of convergence toaccelerometer/magnetometer
#define twoKi 0.005f // integral gain governs rate of convergenceof gyroscope biases
#define halfT 0.0025f // half the sample period
#define ACCEL_1G 1000 //theacceleration of gravity is: 1000 mg
/* Private variables———————————————————*/
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing theestimated orientation
static float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
/* Public variables———————————————————-*/
const float sampleFreq = 1 / (2 * halfT);
float integralFBx = 0.0f; // prevent integral windup
float integralFBy = 0.0f;
float integralFBz = 0.0f;
float invSqrt(float x){
return 1.0 / sqrt(x);
}
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
//如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误。
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
//把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数，使用平方根的倒数而不是直接使用平方根的原因是使得下面的ax，ay，az的运算速度更快。通过归一化处理后，ax，ay，az的数值范围变成 – 1到 + 1之间。
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
//根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正。
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3-q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0-0.5f + q3 * q3;
//使用叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz-az * halfvy);
halfey = (az * halfvx-ax * halfvz);
halfez = (ax * halfvy-ay * halfvx);
//把上述计算得到的重力差进行积分运算，积分的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。积分系数是Ki，如果Ki参数设置为0，则忽略积分运算。
// Compute and apply integral feedback if enabled
if (twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
//把上述计算得到的重力差进行比例运算。比例的结果累加到陀螺仪的数据中，用于修正陀螺仪数据。比例系数为Kp。
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
//通过上述的运算，我们得到了一个由加速计修正过后的陀螺仪数据。接下来要做的就是把修正过后的陀螺仪数据整合到四元数中。
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx-qc * gy-q3 * gz);
q1 += (qa * gx + qc * gz-q3 * gy);
q2 += (qa * gy-qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy-qc * gx);
//把上述运算后的四元数进行归一化处理。得到了物体经过旋转后的新的四元数。
// Normalise quaternion
recipNorm = 1.0/sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
#else
#define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接
float Ts = 200;
float halfT = 0.001f; // 采样周期的一半
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
float Yaw, Pitch, Roll; //偏航角，俯仰角，翻滚角
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df – (i >> 1);
y = *(float*)&i;
y = y * (1.5f – (halfx * y * y));
return y;
}
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
// 测量正常化
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm; //单位化
ay = ay / norm;
az = az / norm;
// 估计方向的重力
vx = 2 * (q1*q3 – q0*q2);
vy = 2 * (q0*q1 + q2*q3);
vz = q0*q0 – q1*q1 – q2*q2 + q3*q3;
// 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
ex = (ay*vz – az*vy);
ey = (az*vx – ax*vz);
ez = (ax*vy – ay*vx);
// 积分误差比例积分增益
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;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 – 2 * q2* q2 + 1)* 57.3; // rollv
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //此处没有价值，注掉
}
#endif