首先，我們澄清一點很多人都在討論的問題：卡爾曼濾波器是不是真的優於互補濾波器？答案是肯定的，但有一些前提：感測器的雜訊是隨機的，服從正態分佈的，我們大概知道雜訊的協方差矩陣；並且「優於」是在最小均方誤差（least mean squared error）的意義上。參考文獻 [1] p.284 中對於互補濾波器和卡爾曼濾波器的優劣有非常好的描述，在這裡做一摘抄：「在很多實際應用中，我們對於一些測量變數的誤差模型很難有準確的估計，……，或者一些誤差不是隨機的或正態分佈的。因此，如果有不需要對測量變數的誤差作任何假設的方法可能會更好，從而避免錯誤的模型帶來的巨大的估計錯誤。這樣的方法在最小均方誤差的意義上可能會有些損失，但它比在一些不通常的情況下因為模型錯誤造成的巨大錯誤要好。」互補濾波器就是一種不需要對誤差模型做過多假設的方法。
1，一維互補濾波器
假設乙個角度訊號
（1）其中
和 是測量雜訊。我們希望對
和 作加權平均，從而使平均後的誤差盡可能最小。在 imu 的姿態估計中，如果我們認為
是陀螺儀積分出的角度，那麼
在短時間內是準確的，但長時間後因為漂移誤差精度會大幅下降，也就是說
主要是低頻雜訊；如果我們認為
是加速度計通過和重力對比解算出的角度，那麼
在短時間內因為運動造成的加速度是不準確的，但 imu 不可能長時間沿乙個方向加速，所以在長時間內的
平均值是準確的，也就是說
主要是高頻雜訊。因此乙個自然的想法是：我們可以把
通過乙個高通濾波器，濾掉低頻雜訊；把
通過乙個低通濾波器，濾掉高頻雜訊，再做平均，那麼得到的結果應該是準確的。這就是互補濾波器的核心想法，也是它為什麼叫「互補」的原因。
如果我們把
和 作 laplace 變換寫成頻域中的訊號，並用
表示低通濾波器，對兩個訊號進行平均可以得到：
（2）其中
是對需要測量的角度訊號
的估計，
是互補的高通濾波器。因為
是角速度積分得到的，因此可以寫成：
（3）其中
是測量得到的包含誤差的角速度。把（3）代入（2）可以得到：
（4）如果我們使用最簡單的一階低通濾波器，也就是
（5）再將（5）作一階積分可以得到：
（6）公式（6）也就是互補濾波器在時域中的表示式。可以看到，互補濾波器實際是對陀螺儀積分得到的角度和加速度計解算出的角度做了加權平均，權重分別是
和 。需要注意的是，
權重是不隨時間變化的，這是互補濾波器和卡爾曼濾波器的本質區別。直觀上講，因為加速計的雜訊特性不隨時間改變，但陀螺儀積分得到的角度隨著濾波的進行會越來越準確，因此在卡爾曼濾波器中
得到的權重會越來越小，這是卡爾曼濾波器優於互補濾波器的原因。這樣簡單的一維互補濾波器在 imu 姿態估計中也有實際應用。例如在兩輪平衡車中，我們只需要估計平衡車傾斜的角度，因此可以把姿態簡化為一維角度，使用加速度計和陀螺儀進行估計。
2，so(3) 上的互補濾波器
在無人機的姿態估計中，乙個十分流行的演算法是 mahony 等人提出的在 so(3) 群上的互補濾波器 [2]。這個濾波器的思路是仿照公式（5）的形式，把其中的加減法替換為 so(3) 群上的運算。
的導數，
，其中
是 imu 的角速度，並且表示在 imu 體座標系中。仿照公式（5），我們需要在
的導數中新增一項：通過陀螺儀積分和其它方式測量得到的姿態的差值，乘以乙個比例係數。如果我們把
的估計記為
，其它方式測量得到的姿態記為
，那麼兩者的「差」在 so(3) 群上可以表示為
（7）通過指數對映的逆對映，可以得到
的軸-角表示
，即乙個
中的量。現在我們可以寫出 so(3) 群上互補濾波器的表示式：
（8）需要注意的是，
之所以可以像（5）中那樣和
相加，是因為它們都是
中的量，而
是乙個線性空間。直觀的解釋是，
和 都可以理解為三維角速度，因此可以相加。在 mahony 的文章中，
被替換成
，其中
是 的逆運算。這個替換是無關緊要的，因為
和 是平行的，而它們的係數都可以被吸收進比例係數
中。公式（8）可以積分為在 imu 姿態估計中可以使用的表示式：
（9）3，總結
在這篇文章中，我們介紹了簡單的一維互補濾波器，和比較複雜的在 so(3) 群上的互補濾波器。互補濾波器的思路是把乙個主要包含高頻雜訊，和乙個主要包含低頻雜訊的訊號分別通過乙個低通濾波器和高通濾波器，並做平均，從而使平均後的結果是真實訊號較為準確的估計。雖然 mahony 提出的互補濾波器在無人機領域十分流行，但在最小均方誤差的意義下，它是劣於卡爾曼濾波器的。從下篇文章開始，我們逐步介紹卡爾曼濾波器在 imu 姿態估計中的應用。
參考文獻
[2] mahony, robert, tarek hamel, and jean-michel pflimlin. "nonlinear complementary filters on the special orthogonal group."ieee transactions on automatic control53.5 (2008): 1203-1217.
互補濾波係數 從全狀態觀測器到卡爾曼濾波器（二）
本文使用 zhihu on vscode 創作並發布 寫在開頭從全狀態觀測器到卡爾曼濾波器 一 從全狀態觀測器到卡爾曼濾波器 二 從全狀態觀測器到卡爾曼濾波器 三 從全狀態觀測器到卡爾曼濾波器 四 我們現在希望用乙個超聲波測距感測器測量固定在直線軌道上的小車位置 其中為時刻測量值，為時刻溫度，為為時...
關於互補濾波原理
先介紹一下互補濾波的基本概念，這是阿莫論壇上乙個會員的總結 對mpu6050來說，加速度計對四軸或小車的加速度比較敏感，取瞬時值計算傾角誤差比較大 而陀螺儀積分得到的角度不受小車加速度的影響，但是隨著時間的增加積分漂移和溫度漂移帶來的誤差比較大。所以這兩個感測器正好可以彌補相互的缺點。不過要怎麼彌補...
一階互補濾波
六軸採集原始資料用的都是16位的adc，所以顯示的數字是從 32768 32768，要看自己選擇的量程進行換算，量程選擇在配置裡面找，比如選擇 250 250量程，那麼 32768 32768就對應 250 250 進行換算就可以了。對mpu6050來說，加速度計對四軸或小車的加速度比較敏感，取瞬時...