6軸陀螺儀
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){
}
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)
//把加速度計的資料進行歸一化處理。其中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