四元数

圆点博士小四轴之四元数算法。四元数算法在小四轴上一般直接采用Madgwick的算法就差不多。

这是一个居于GPL的协议，大家可以自由使用。

//================

// IMU.c

// S.O.H. Madgwick

// 25th September 2010

//=================

转载如下：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {

float norm;

float vx, vy, vz;

float ex, ey, ez;

// normalise the measurements

norm = sqrt(ax*ax + ay*ay + az*az);

ax = ax / norm;

ay = ay / norm;

az = az / norm;

// estimated direction of gravity

vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3);

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

// error is sum of cross product between reference direction of field and direction measured by sensor

ex = (ay*vz - az*vy);

ey = (az*vx - ax*vz);

ez = (ax*vy - ay*vx);

// integral error scaled integral gain

exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki;

ezInt = ezInt + ez*Ki;

// adjusted gyroscope measurements

gx = gx + Kp*ex + exInt;

gy = gy + Kp*ey + eyInt;

gz = gz + Kp*ez + ezInt;

// integrate quaternion rate and normalise

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;

// normalise quaternion

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

}

互补滤波

在小四轴上，我们常见的滤波方法是互补滤波。下面我们来看互补滤波是怎么工作的。

根据下图，我们可以知道，

我们每次得到的角度数据是由下列成分组成的：

1。当前的角度

2。当前陀螺仪运动所产生的角度

3。当前物体运动所产生的加速度角度

通过改变系数a和b, 我们能够调整上述各个成分的权重值，从而得到不同的滤波结果。通常我们采用a>b来进行运算。即当前的角度和陀螺仪运动产生的角度占有更大的比重，而当前加速度角度占有比较小的比重。

根据该理论，我们来看圆点博士小四轴代码中的实现方法：

bs004_mpu6050_acc_pitch_com=

(bs004_filter_high*(bs004_mpu6050_acc_pitch_com+bs004_mpu6050_gyro_pitch_raw*bs004_filter_time)

+bs004_filter_low*bs004_mpu6050_acc_pitch_raw)

/(bs004_filter_high+bs004_filter_low);

bs004_mpu6050_acc_roll_com=

(bs004_filter_high*(bs004_mpu6050_acc_roll_com +bs004_mpu6050_gyro_roll_raw *bs004_filter_time)

+bs004_filter_low*bs004_mpu6050_acc_roll_raw)

/(bs004_filter_high+bs004_filter_low);