四元數+互補濾波

四元數

圓點博士小四軸之四元數算法。四元數算法在小四軸上一般直接采用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;
}

 

來源： <http://blog.sina.com.cn/s/blog_da6fea910101d1ya.html>

 

互補濾波

在小四軸上，我們常見的濾波方法是互補濾波。下面我們來看互補濾波是怎么工作的。

根據下圖，我們可以知道，

我們每次得到的角度數據是由下列成分組成的：

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);

來源： <http://blog.sina.com.cn/s/blog_da6fea910101d1r2.html>

 

來自為知筆記(Wiz)