You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
58 lines
2.1 KiB
58 lines
2.1 KiB
3 years ago
|
#define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计
|
||
|
|
#define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接
|
||
|
|
#define halfT 0.001f // 采样周期的一半
|
||
|
|
|
||
|
|
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向
|
||
|
|
float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差
|
||
|
|
|
||
|
|
float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角
|
||
|
|
|
||
|
|
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; //此处没有价值，注掉
|
||
|
|
}
|