先介紹一下互補濾波的基本概念，這是阿莫論壇上一個會員的總結：對mpu6050來說，加速度計對四軸或小車的加速度比較敏感，取瞬時值計算傾角誤差比較大；而陀螺儀積分得到的角度不受小車加速度的影響，但是隨著時間的增加積分漂移和溫度漂移帶來的誤差比較大。所以這兩個傳感器正好可以彌補相互的缺點。不過要怎么彌補呢？經過上面的介紹是否感覺到可以用濾波器做文章呢？這里講的互補濾波就是在短時間內采用陀螺儀得到的角度做為最優，定時對加速度采樣來的角度進行取平均值來校正陀螺儀的得到的角度。就是，短時間內用陀螺儀比較準確，以它為主；長時間用加速度計比較準確，這時候加大它的比重，這就是互補了，不過濾波在哪里？加速度計要濾掉高頻信號，陀螺儀要濾掉低頻信號，互補濾波器就是根據傳感器特性不同，通過不同的濾波器（高通或低通，互補的），然后再相加得到整個頻帶的信號，例如，加速度計測傾角，其動態響應較慢，在高頻時信號不可用，所以可通過低通抑制高頻；陀螺響應快，積分后可測傾角，不過由于零漂等，在低頻段信號不好。通過高通濾波可抑制低頻噪聲。將兩者結合，就將陀螺和加表的優點融合起來，得到在高頻和低頻都較好的信號，互補濾波需要選擇切換的頻率點，即高通和低通的頻率。 鑒于加速度計低頻特性比較好，因為加速度的角度可以直接算出來，沒有累積誤差，所以長時間后也比較準。而陀螺儀長時間后由于積分誤差的累加，會造成輸出誤差比較大，甚至無法使用。所以用互補濾波法根據他們的特性取長補短進行姿態解算，每過一段時間就讓加速度計去校準一下陀螺儀。互補濾波就是在短時間內采用陀螺儀得到的角度做為最優值，定時對加速度采樣來的加速度值進行取平均值來校正陀螺儀的得到的角度。短時間內用陀螺儀比較準確，以它為主；長時間用加速度計比較準確，這時候加大它的比重，這就是互補了，不過加速度計要濾掉高頻信號，陀螺儀要濾掉低頻信號，互補濾波器就是根據傳感器特性不同，通過不同的濾波器（高通或低通，互補的），然后再相加得到整個頻帶的信號。互補是給他們不同的權重加權求和。
互補濾波是這樣的：陀螺積分角度+=角速度*dt；融合角度=陀螺權值*陀螺積分角度+(1-陀螺權值)*加速度角度；還有一種叫最大梯度法的，就是你發的這段代碼
// 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;大概是這樣的： 陀螺積分角度+=角速度*dt；角度偏差=加速度角度-陀螺積分角度；融合角度=陀螺角度+衰減系數*角度偏差；用來矯正陀螺本身的靜態漂移：角度偏差積分+=角度偏差；角速度=角速度+衰減系數*角度偏差積分；這個就可以一定程度上矯正陀螺的零漂。 下面這幅圖才是準確的闡述了互補濾波的過程。正常情況下用陀螺儀的數據就可以進行姿態的更新，但是由于陀螺儀的積分誤差，這里用acc和mag去校正，求出他們的誤差用PI去彌補。注意看看pid的公式和作用，pid是作用于誤差（實際個期望之間的差值），最終反復調節，讓實際值=期望值。
互補濾波——加速度計濾掉高頻信號，陀螺儀濾掉低頻信號，互補濾波器就是根據傳感器特性不同，通過不同的濾波器（高通或低通，互補的），然后再相加得到整個頻帶的信號。
參考博客 http://blog.csdn.net/wkdwl/article/details/52119163http://blog.csdn.net/Gen_Ye/article/details/52522721?locationNum=10http://blog.csdn.net/Gen_Ye/article/details/52524190 新聞熱點
疑難解答
