// This function is borrowed from the Android reference
// at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
// It calculates a rotation vector from the gyroscope angular speed values.
void getRotationVectorFromGyro(double deltaRotationVector[4], double dT)
{
static const double EPSILON = 0.000000001f;
double normValues[3] = {0.0, 0.0, 0.0};
// Calculate the angular speed of the sample (squared).
double omegaMagnitude_sq = gyro[0] * gyro[0] + gyro[1] * gyro[1] + gyro[2] * gyro[2];
// Normalize the rotation vector if it's big enough to get the axis
if (omegaMagnitude_sq > EPSILON) {
double recipNorm = invSqrt(omegaMagnitude_sq);
normValues[0] = gyro[0] * recipNorm;
normValues[1] = gyro[1] * recipNorm;
normValues[2] = gyro[2] * recipNorm;
}
// Integrate around this axis with the angular speed by the timestep
// in order to get a delta rotation from this sample over the timestep
// We will convert this axis-angle representation of the delta rotation
// into a quaternion before turning it into the rotation matrix.
double thetaOverTwo = sqrt(omegaMagnitude_sq) * dT / 2.0f;
double sinThetaOverTwo = sin(thetaOverTwo);
double cosThetaOverTwo = cos(thetaOverTwo);
deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
deltaRotationVector[3] = cosThetaOverTwo;
}
void main(void)
{
//short x[1024],y[1024];
int i;
for (i=0;i<32768;i++)
printf("1/sqrt(%d) = %d\n",i,invSqrt(i));
}
float norm(const matrix<N, 1>& vec)
{
float acc = 0;
for(unsigned int i = 0; i < N; ++i)
acc += vec[i] * vec[i];
return 1/invSqrt(acc);
}
ci::Vec3f AllignmentRule::getSteer( Boid* const b )
{
m_Boid = b;
ci::Vec3f sum = ci::Vec3f::zero();
int count = 0;
std::list<Boid>* boidList;
boidList = m_AppController->getBoidList();
std::list<Boid>::iterator boidIter = boidList->begin();
for ( ; boidIter != boidList->end(); ++boidIter )
{
ci::Vec3f diff = m_Boid->m_pos - boidIter->m_pos;
float lengthSq = diff.lengthSquared();
if ( ( lengthSq > 0 ) && ( lengthSq < DESIRED_ALI_SQ) )
{
sum += boidIter->m_vel;
count++;
}
}
#ifdef USE_FAST_INV_SQRT
if (count > 0)
{
sum /= (float)count;
float lengthSq = sum.lengthSquared();
if ( lengthSq > 0 )
{
sum*=invSqrt( lengthSq );
}
sum*=MAX_SPEED;
ci::Vec3f steer = sum - m_Boid->m_vel;
steer.limit(MAX_FORCE);
return steer;
} else {
return ci::Vec3f::zero();
}
#else
if (count > 0)
{
sum /= (float)count;
float length = sum.length();
if ( length > 0 )
{
sum/=length;
}
sum*=MAX_SPEED;
ci::Vec3f steer = sum - m_Boid->m_vel;
steer.limit(MAX_FORCE);
return steer;
} else {
return ci::Vec3f::zero();
}
#endif
}
void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
#elif IS_6DOM()
void FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az) {
#endif
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
float qa, qb, qc;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
#if IS_9DOM() && not defined(DISABLE_MAGN)
// Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
float hx, hy, bx, bz;
float halfwx, halfwy, halfwz;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (my * halfwz - mz * halfwy);
halfey = (mz * halfwx - mx * halfwz);
halfez = (mx * halfwy - my * halfwx);
}
#endif
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
float halfvx, halfvy, halfvz;
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (ay * halfvz - az * halfvy);
halfey += (az * halfvx - ax * halfvz);
halfez += (ax * halfvy - ay * halfvx);
}
// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
// 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;
}
// 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 = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useYaw, float yawError)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float hx, hy, bx;
float ex = 0, ey = 0, ez = 0;
float qa, qb, qc;
// Calculate general spin rate (rad/s)
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else)
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
ez += sin_approx(yawError / 2.0f);
}
// Use measured magnetic field vector
recipNorm = sq(mx) + sq(my) + sq(mz);
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex += rMat[2][0] * ez_ef;
ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef;
}
// Use measured acceleration vector
recipNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipNorm > 0.01f) {
// Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex += (ay * rMat[2][2] - az * rMat[2][1]);
ey += (az * rMat[2][0] - ax * rMat[2][2]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
}
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
float dcmKiGain = imuRuntimeConfig->dcm_ki;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
}
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
// Apply proportional and integral feedback
gx += dcmKpGain * ex + integralFBx;
gy += dcmKpGain * ey + integralFBy;
gz += dcmKpGain * ez + integralFBz;
// Integrate rate of change of quaternion
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
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 = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
}
/*
* This method uses data from gyroscope, accelerometer and magnetometer to estimate
* the position of a spacecraft. Uses a variation of Sebastian Madgwicks filter,
* See:
* "An efficient orientation filter for inertial and inertial/magnetic sensor arrays",
* Sebastian Madgwicks, April 2010.
*
* The original code comes from:
* http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
*
* Includes an updated version of the gradient descend algorithm.
* See:
* http://diydrones.com/forum/topics/madgwick-imu-ahrs-and-fast-inverse-square-root?id=705844%3ATopic%3A1018435&page=4#comments
* Jeroen van de Mortel, August 2014
*
* Changes were made by me to avoid repeated calculations and speed up the filter
*
* Uses a fast inverse square root implementation.
*
* The gyroscope measurements need to be scaled to rad/s, the accelerometer and magnetometer data
* does not need to be properly scaled but should to be calibrated. This holds especially for the
* magnetometer values, a simple offset/scale model might not be sufficient
*
* Make sure that the coordinate systems of the individual sensors are correctly aligned.
*
* For the magnetometer calibration an ellipsoid fit algorithm might be useful.
* See: http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
* Yury Petrov, September 2015
*
* @params: float gx, gy, gz: The gyroscope measurements in rad/s
* float ax, ay, az: The accelerometer values
* float mx, my, mz: The magnetometer values
*/
void MadgwickFilter::filterUpdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float scale; // Used to scale xyz values to length 1
/* Rate of change of quaternion from gyroscope */
float qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
float qDot2 = 0.5f * ( q0 * gx + q2 * gz - q3 * gy);
float qDot3 = 0.5f * ( q0 * gy - q1 * gz + q3 * gx);
float 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 */
scale = invSqrt(ax * ax + ay * ay + az * az);
ax *= scale;
ay *= scale;
az *= scale;
/* Normalise magnetometer measurement */
scale = invSqrt(mx * mx + my * my + mz * mz);
mx *= scale;
my *= scale;
mz *= scale;
// Auxiliary variables to avoid repeated arithmetic
float _2q0mx = 2.0f * q0 * mx;
float _2q0my = 2.0f * q0 * my;
float _2q0mz = 2.0f * q0 * mz;
float _2q1mx = 2.0f * q1 * mx;
float _2q0 = 2.0f * q0;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q0q2 = 2.0f * q0 * q2;
float _2q2q3 = 2.0f * q2 * q3;
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;
// Reference direction of Earth's magnetic field
float hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
float hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
float _2bx = sqrt(hx * hx + hy * hy);
float _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
float _4bx = 2.0f * _2bx;
float _4bz = 2.0f * _2bz;
float _8bx = 2.0f * _4bx;
float _8bz = 2.0f * _4bz;
// Gradient decent algorithm corrective step
float t0 = 0.5 - q1q1 - q2q2;
float t1 = (2.0f*(q1q3 - q0q2) - ax);
float t2 = (2.0f*(q0q1 + q2q3) - ay);
float t3 = 4.0f * (2.0f*t0 - az);
float t4 = (_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx);
float t5 = _4bx*(q0q2 + q1q3) + _4bz*t0 - mz;
float t6 = _4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my;
float s0 = -_2q2 * t1 + _2q1 * t2 - _4bz * q2 * t4 + ( -_4bx*q3 + _4bz*q1) * t6 + _4bx * q2 * t5;
float s1 = _2q3 * t1 + _2q0 * t2 - q1 * t3 + _4bz * q3 * t4 + ( _4bx*q2 + _4bz*q0) * t6 + (_4bx * q3 - _8bz * q1) * t5;
float s2 = -_2q0 * t1 + _2q3 * t2 - q2 * t3 + (-_8bx * q2 - _4bz * q0) * t4 + ( _4bx*q1 + _4bz*q3) * t6 + (_4bx * q0 - _8bz * q2) * t5;
float s3 = _2q1 * t1 + _2q2 * t2 + (-_8bx * q3 + _4bz * q1) * t4 + ( -_4bx*q0 + _4bz*q2) * t6 + _4bx * q1 * t5;
// Normalize step magnitude
scale = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= scale;
s1 *= scale;
s2 *= scale;
s3 *= scale;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * interval;
q1 += qDot2 * interval;
q2 += qDot3 * interval;
q3 += qDot4 * interval;
// Normalize quaternion
scale = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= scale;
q1 *= scale;
q2 *= scale;
q3 *= scale;
}
void Madgwick::update(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, _8bx, _8bz, _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)) {
updateIMU(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;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
_8bx = 2.0f * _4bx;
_8bz = 2.0f * _4bz;
// Gradient decent algorithm corrective step
s0= -_2q2*(2.0f*(q1q3 - q0q2) - ax) + _2q1*(2.0f*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2.0f*(q1q3 - q0q2) - ax) + _2q0*(2.0f*(q0q1 + q2q3) - ay) + -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s2= -_2q0*(2.0f*(q1q3 - q0q2) - ax) + _2q3*(2.0f*(q0q1 + q2q3) - ay) + (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2.0f*(q1q3 - q0q2) - ax) + _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// 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;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
int accWeight, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useCOG, float courseOverGround)
{
static float integralAccX = 0.0f, integralAccY = 0.0f, integralAccZ = 0.0f; // integral error terms scaled by Ki
static float integralMagX = 0.0f, integralMagY = 0.0f, integralMagZ = 0.0f; // integral error terms scaled by Ki
float recipNorm;
float ex, ey, ez;
float qa, qb, qc;
/* Calculate general spin rate (rad/s) */
float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
/* Step 1: Yaw correction */
// Use measured magnetic field vector
if (useMag || useCOG) {
float kpMag = imuRuntimeConfig->dcm_kp_mag * imuGetPGainScaleFactor();
recipNorm = mx * mx + my * my + mz * mz;
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles
// (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
float bx = sqrtf(hx * hx + hy * hy);
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
float ez_ef = -(hy * bx);
// Rotate mag error vector back to BF and accumulate
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
else if (useCOG) {
// Use raw heading error (from GPS or whatever else)
while (courseOverGround > M_PIf) courseOverGround -= (2.0f * M_PIf);
while (courseOverGround < -M_PIf) courseOverGround += (2.0f * M_PIf);
// William Premerlani and Paul Bizard, Direction Cosine Matrix IMU - Eqn. 22-23
// (Rxx; Ryx) - measured (estimated) heading vector (EF)
// (cos(COG), sin(COG)) - reference heading vector (EF)
// error is cross product between reference heading and estimated heading (calculated in EF)
float ez_ef = - sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0];
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
else {
ex = 0;
ey = 0;
ez = 0;
}
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki_mag > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralMagX += imuRuntimeConfig->dcm_ki_mag * ex * dt; // integral error scaled by Ki
integralMagY += imuRuntimeConfig->dcm_ki_mag * ey * dt;
integralMagZ += imuRuntimeConfig->dcm_ki_mag * ez * dt;
gx += integralMagX;
gy += integralMagY;
gz += integralMagZ;
}
}
// Calculate kP gain and apply proportional feedback
gx += kpMag * ex;
gy += kpMag * ey;
gz += kpMag * ez;
}
/* Step 2: Roll and pitch correction - use measured acceleration vector */
if (accWeight > 0) {
float kpAcc = imuRuntimeConfig->dcm_kp_acc * imuGetPGainScaleFactor();
// Just scale by 1G length - That's our vector adjustment. Rather than
// using one-over-exact length (which needs a costly square root), we already
// know the vector is enough "roughly unit length" and since it is only weighted
// in by a certain amount anyway later, having that exact is meaningless. (c) MasterZap
recipNorm = 1.0f / GRAVITY_CMSS;
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
float fAccWeightScaler = accWeight / (float)MAX_ACC_SQ_NEARNESS;
// Error is sum of cross product between estimated direction and measured direction of gravity
ex = (ay * rMat[2][2] - az * rMat[2][1]) * fAccWeightScaler;
ey = (az * rMat[2][0] - ax * rMat[2][2]) * fAccWeightScaler;
ez = (ax * rMat[2][1] - ay * rMat[2][0]) * fAccWeightScaler;
// Compute and apply integral feedback if enabled
if(imuRuntimeConfig->dcm_ki_acc > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) {
integralAccX += imuRuntimeConfig->dcm_ki_acc * ex * dt; // integral error scaled by Ki
integralAccY += imuRuntimeConfig->dcm_ki_acc * ey * dt;
integralAccZ += imuRuntimeConfig->dcm_ki_acc * ez * dt;
gx += integralAccX;
gy += integralAccY;
gz += integralAccZ;
}
}
// Calculate kP gain and apply proportional feedback
gx += kpAcc * ex;
gy += kpAcc * ey;
gz += kpAcc * ez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * dt);
gy *= (0.5f * dt);
gz *= (0.5f * dt);
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 = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
// Pre-compute rotation matrix from quaternion
imuComputeRotationMatrix();
}
void MayhonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfex, halfey, halfez;
float qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// find inverse squareroot of accelerometer data
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
//gain scheduling for measured acceleration
if((recipNorm>0.0017755f)&&(recipNorm<0.00217f)) //only use accelerometer if normal is between 1.1 and 0.9G
{
currentT = millis();
if(currentT < 5000) //give AHRS time to converge to initial conditions
{
f.OK_TO_ARM = 0;
f.ARMED = 0;
if(currentT > convergedT)
{
LEDPIN_TOGGLE;
convergedT = currentT +50;
}
twoKp = 40;//accelerate AHRS towards initial conditions while multicopter is held steady
}
else//normal operation
{
twoKp = twoKpDef;
}
}
else
{
twoKp = twoKpDefMin; //reduce proportional gain to reduce effects of erroneous corrections in pitch and roll
}
//Normalise accelerometer reading
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);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * samplePeriod; // integral error scaled by Ki
integralFBy += twoKi * halfey * samplePeriod;
integralFBz += twoKi * halfez * samplePeriod;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * samplePeriod); // pre-multiply common factors
gy *= (0.5f * samplePeriod);
gz *= (0.5f * samplePeriod);
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 = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
//convert to tait bryan angles and store
getAttitude();
}
void MayhonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfwx, halfwy, halfwz;
float halfex, halfey, halfez;
float qa, qb, qc;
uint32_t currentT;
//save time at current iteration
currentT = millis();
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return;
}
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
//compute inverse squareroot of accelerometer readings
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
//gain scheduling for feedback based on external accelerations
if((recipNorm>0.0017755f)&&(recipNorm<0.00217f)) //high gain if normal is between 1.1 and 0.9G
{
if(currentT < 5000) //give AHRS time to converge to initial conditions
{
f.OK_TO_ARM = 0;
f.ARMED = 0;
if(currentT > convergedT)
{
LEDPIN_TOGGLE;
convergedT = currentT +50;
}
twoKp = 40;//accelerate AHRS towards initial conditions while multicopter is held steady
}
else if(currentT > accelT)//normal operation
{
twoKp = twoKpDef;
twoKi = twoKiDef;
}
}
else
{
accelT = currentT + 500; //reduce gains for 200ms after acceleration detected
twoKp = twoKpDefMin; //reduce proportional gain to reduce effects of erroneous corrections in pitch and roll
twoKi = 0; //reset integral to prevent windup and remove unwanted effects of large accel errors
}
// Normalise accelerometer measurement
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field, finding how much inclination should exist by translating to estimated earth frame
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
// Estimated direction of gravity and magnetic field
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); //translate the reading back into sensor frame
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// Error is weighted sum of cross product between estimated direction and measured direction of field vectors
halfex = 2*(AccFactor*(ay * halfvz - az * halfvy) + MagFactor*(my * halfwz - mz * halfwy))/(AccFactor+MagFactor);//reduce cross coupling in pitch and roll
halfey = 2*(AccFactor*(az * halfvx - ax * halfvz) + MagFactor*(mz * halfwx - mx * halfwz))/(AccFactor+MagFactor);
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);//more emphasis in compass yaw rotation
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * samplePeriod; // integral error scaled by Ki
integralFBy += twoKi * halfey * samplePeriod;
integralFBz += twoKi * halfez * samplePeriod;
constrain(integralFBx,-0.008f,0.008f); //constrain integral error to 0.5 degs/sample to prevent integral windup
constrain(integralFBy,-0.008f,0.008f); //(roughly error of 1 rad for 8/gain milliseconds to saturate = 800milliseconds at 0.01 ki)
constrain(integralFBz,-0.008f,0.008f);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += halfex*twoKp;
gy += halfey*twoKp;
gz += halfez*twoKp;
}
// Integrate rate of change of quaternion
gx *= (0.5f * samplePeriod); // pre-multiply common factors
gy *= (0.5f * samplePeriod);
gz *= (0.5f * samplePeriod);
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 = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
//convert to tait bryan angles and store
getAttitude();
}
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;
// 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;
// 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);
// 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;
}
// 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 = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
float Plane3::distanceTo(glm::vec3 point)
{
return (a*point.x + b*point.y + c*point.z + d)*invSqrt(a*a + b*b + c*c);
}
void ImuFilter::madgwickAHRSupdate(
float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz,
float dt)
{
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;
float _w_err_x, _w_err_y, _w_err_z;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if (std::isnan(mx) || std::isnan(my) || std::isnan(mz))
{
madgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az, dt);
return;
}
// 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;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// compute gyro drift bias
_w_err_x = _2q0 * s1 - _2q1 * s0 - _2q2 * s3 + _2q3 * s2;
_w_err_y = _2q0 * s2 + _2q1 * s3 - _2q2 * s0 - _2q3 * s1;
_w_err_z = _2q0 * s3 - _2q1 * s2 + _2q2 * s1 - _2q3 * s0;
w_bx_ += _w_err_x * dt * zeta_;
w_by_ += _w_err_y * dt * zeta_;
w_bz_ += _w_err_z * dt * zeta_;
gx -= w_bx_;
gy -= w_by_;
gz -= w_bz_;
// 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);
// Apply feedback step
qDot1 -= gain_ * s0;
qDot2 -= gain_ * s1;
qDot3 -= gain_ * s2;
qDot4 -= gain_ * s3;
}
else{
// 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);
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}