《The Balance Filter》互補濾波器--MIT著名牛文翻譯（下）

      鄙人在寫另一篇博文時頻頻借鑑到這篇牛文（實際上是一個PPT），爲能讓更多人方便查閱，共同進步、探討，遂翻譯全文。鄙人才疏學淺，願附上原文對照，以期指正。首發於CSDN：http://blog.csdn.net/qq_32666555。轉載請註明作者及出處，謝謝！

The Balance Filter——A Simple Solution for Integrating Accelerometer and Gyroscope Measurements for a Balancing Platform

互補濾波器—— 一種用於以加速度計和陀螺儀坐整合測量的平衡平臺的簡單解決方案

作者：Shane Colton

譯者：Wyman

Mapping Sensors

設計傳感器

最簡單粗暴的

Pros:

• Intuitive.

• Easy to code.

• Gyro gives fast and accurate angular velocity measurement.

利：

• 直觀。

• 易於編程。

• 陀螺儀反映迅速，測量角速度準確。

Cons:

• Noisy.

• X-axis will read any horizontal acceleration as a change in angle. (Imagine the platform is horizontal, but the motors are causing it to accelerate forward. The accelerometer cannot distinguish this from gravity.)

弊：

• 噪聲大

• X軸將任何水平加速度當作角度改變。（想像水平的平臺在電機使其向前加速時，加速度計不能分辨它和重力。）

快速粗糙修復（濾波）

*Could be as simple as averaging samples:

angle= (0.75)*(angle) + (0.25)*(x_acc);

0.75 and 0.25 are example values. These could be tuned to change the time constant of the filter as desired.

* 簡單如加權平均：

angle= (0.75)*(angle) + (0.25)*(x_acc);

0.75 和 0.25 是舉個栗子。 可以通過調諧它們來改變濾波器期望的時間常量。

Pros:

• Still Intuitive.

• Still easy to code.

• Filters out short-duration horizontal accelerations. Only long-term acceleration (gravity) passes through.

利：

• 依然直觀。

• 依然易於編程。

• 濾除高頻水平加速度噪聲。只有低頻加速度（重力）可通過。

Cons:

• Angle measurement will lag due to the averaging. The more you filter, the more it will lag. Lag is generally bad for stability.

弊：

• 角度測量會因爲加權平均計算而滯後。濾波越多，滯後越大。滯後通常有害於穩定性。

單傳感器法

*Simple physics, dist.=vel. × time. Accomplished in code like this:

angle= angle + gyro * dt;

Requires that you know the time interval between updates, dt.

* 簡單物理式，積分量 = 速度 × 時間。代碼可以麻溜地碼成這樣：

angle= angle + gyro * dt;

你需要知道採樣間隔時間，dt。

Pros:

• Only one sensor to read.

• Fast, lag is not a problem.

• Not subject to horizontal accelerations.

• Still easy to code.

利：

• 只需讀取一個傳感器。

• 快速，間隔不是問題。

• 不受制於水平加速度。

• 依然易於編程。

Cons:

• The dreaded gyroscopic drift. If the gyro does not read perfectly zero when stationary (and it won’t), the small rate will keep adding to the angle until it is far away from the actual angle.

弊：

• 可怕的陀螺儀漂移。如果陀螺儀靜止零點採集不好（實際如此），小誤差會一直加到角度直至其遠偏離於真實的角度。

卡爾曼濾波器

Pros:

• Supposedly the theoretically-ideal filter for combining noisy sensors to get clean,accurate estimates.

• Takes into account known physical properties of the system (mass, inertia, etc.).

利：

• 講道理，理想的濾波器可以融合帶有噪聲的傳感器並得到乾淨、精確的估值。

• 考慮了系統已知的物理特性（質量、慣性等）

Cons:

• I have no idea how it works. It’s mathematically complex, requiring some knowledge of linear algebra. There are different forms for different situations, too.

• Probably difficult to code.

• Would kill processor time.

弊：

• 我不知道它咋整滴。他算術複雜，需要一些線性代數的知識。不同情況也有不同形式。

• 應該難以編程。

• 耗費線程時間。

互補濾波器

*Luckily,it’s more easily-said in code:

angle= (0.98)*(angle + gyro * dt) +(0.02)*(x_acc);

More explanation to come…  

* 幸運的是，代碼勝於雄辯：

angle= (0.98)*(angle + gyro * dt) +(0.02)*(x_acc);

下面會接着BB...

Pros:

• Can help fix noise, drift, and horizontal acceleration dependency.

• Fast estimates of angle, much less lag than low-pass filter alone.

• Not very processor-intensive.

利：

• 可以解決噪聲、漂移和水平加速度問題。

• 快速估算角度，滯後比單低通濾波器少很多。

• 不太佔用線程時間。

Cons:

• A bit more theory to understand than the simple filters, but nothing like the Kalman filter.

弊：

• 需要比簡單濾波器懂多點理論，但不像卡爾曼濾波那種騷東西。

More on Digital Filters

數字濾波器進階

There is a lot of theory behind digital filters, most of which I don’t understand,but the basic concepts are fairly easy to grasp without the theoretical notation (z-domain transfer functions,if you care to go into it). Here are some definitions:

Integration: This is easy. Think of a car traveling with a known speed and your program is a clock that ticks once every few milliseconds. To get the new position at each tick, you take the old position and add the change in position. The change in position is just the speed of the car multiplied by the time since the last tick, which you can get from the timers on the microcontroller or some other known timer. In code:

position+= speed*dt;, or for a balancing platform, angle += gyro*dt;.

Low-Pass Filter: The goal of the low-pass filter is to only let through long-term changes, filtering out short-term fluctuations. One way to do this is to force the changes to build up little by little in subsequent times through the program loop. In code:

angle= (0.98)*angle + (0.02)*x_acc;

If,for example, the angle starts at zero and the accelerometer reading suddenly jumps to 10º, the angle estimate changes like this in subsequent iterations:

Iter.	1	2	3	4	5	6	7	8	9	10
θ	0.20º	0.40º	0.59º	0.78º	0.96º	1.14º	1.32º	1.49º	1.66º	1.83º

If the sensor stays at 10º, the angle estimate will rise until it levels out at that value. The time it takes to reach the full value depends on both the filter constants (0.98 and 0.02 in the example) and the sample rate of the loop(dt).

有很多理論支撐數字濾波器，大部分我不懂，不過沒有理論基礎（Z變換，想看可看）基本概念也是很容易抓的。下面給些定義：

積分：這個很簡單。想象一輛以已知速度跑的車，你的程序就是一個一毫秒敲一次的鐘。爲了得到每次的新位置，你取前一次的位置並加上改變量。位置改變量等於車速乘以從上次到現在的時間，這個時間你可以從控制器的定時器或者其它已知的定時器上得到。代碼如下：

position+= speed*dt;, 若是一個平衡平臺 angle += gyro*dt;.

低通濾波器：低通濾波器的目標是隻讓低頻的改變通過，濾掉高頻的波動。實現它的一種方法是在程序的循環中後面的時間一點一點地促使改變，代碼如下：

angle= (0.98)*angle + (0.02)*x_acc;

比如，如果角度從零開始改變，加速度的值突然跳到10°，在隨後的迭代過程中，角度的估計在後面的迭代將如下表所示改變：

迭代	1	2	3	4	5	6	7	8	9	10
角度	0.20º	0.40º	0.59º	0.78º	0.96º	1.14º	1.32º	1.49º	1.66º	1.83º

如果傳感器保持在10°，角度的估計將會上升到和這個值持平，到達這個值所需時間取決於濾波器常數（例子中爲0.98和0.02）和循環中的採樣率(dt)。

High-Pass Filter: The theory on this is a bit harder to explain than the low-pass filter, but conceptually it does the exact opposite: It allows short-duration signals topass through while filtering out signals that are steady over time. This can beused to cancel out drift.

Sample Period: The amount of time that passes between each program loop. If  the sample rate is 100Hz, the sample period is 0.01 sec.

Time Constant: The time constant of a filter is the relative duration of signal it will act on.For a low-pass filter, signals much longer than the time constant pass through unaltered while signals shorter than the time constant are filtered out. The opposite is true for a high-pass filter. The time constant, τ, of a digital low-pass filter,

y= (a)*(y) + (1-a)*(x);,

running in a loop with sample period, dt, can be found like this*:

So if you know the desired time constant and the sample rate, you can pick the filter coefficient a.

Complementary: This just means the two parts of the filter always add to one, so that the output is an accurate, linear estimate in units that make sense. After reading a bit more, I think the filter presented here is not exactly complementary, but is a very good approximation when the time constant is much longer than the sample rate (a necessary condition of digital control anyway)

.高通濾波器：此者理論比低通濾波器要難解釋一些，但是概念上它倆是完全相反的：它允許高頻信號通過，濾去一直保持不變的信號，這能用於消除漂移。

採樣週期：每個循環時間。如果採樣頻率是100Hz，採樣週期是0.01秒。

時間常數：濾波器的時間常數和濾波信號的持續時間相關。對於低通濾波器，比時間常數更長的信號將會保持不變地通過，而比時間常數短的信號將會被濾掉。高通濾波器則相反。一個數字低通濾波器的常數τ：

y= (a)*(y) + (1-a)*(x);,

以採樣週期dt運行的循環，如下：

所以如果已知期望的時間常數和採樣頻率，你可以算出濾波器係數a。

互補：這只是說濾波器總是合二爲一，以確保得到準確、線性的單位內估計。在深入閱讀後，我認爲這裏的濾波器不是單純的互補，但在時間常數比採樣頻率更大的時候是一個很好的近似（一個數控的必要條件）。

A Closer Look at the Angle Complementary Filter

細窺角度互補濾波器

If this filter were running in a loop that executes 100 times per second, the time constant for both the low-pass and the high-pass filter would be:

如果這個濾波器在每秒執行100次的循環中運行，低通和高通濾波器的時間常量都會是：

This defines where the boundary between trusting the gyroscope and trusting the accelerometer is. For time periods shorter than half a second, the gyroscope integration takes precedence and the noisy horizontal accelerations are filtered out. For time periods longer than half a second, the accelerometer average is given more weighting than the gyroscope, which may have drifted by this point. 

這定義了信任陀螺儀還是加速度計的界限。對於時間週期小於半秒，陀螺儀的積分將會佔優，水平加速度的噪聲將會被濾去。對於時間週期長於半秒，加速度的均值相對於陀螺儀佔得比重更大，這時可能會有漂移。

For the most part, designing the filter usually goes the other way. First, you picka time constant and then use that to calculate filter coefficients. Picking the time constant is the place where you can tweak the response. If your gyroscope drifts on average 2º per second (probably a worst-case estimate), you probably want a time constant less than one second so that you can be guaranteed never to have drifted more than a couple degrees in either direction. But the lower the time constant, the more horizontal acceleration noise will be allowed to pass through. Like many other control situations, there is a trade off and the only way to really tweak it is to experiment.

Remember that the sample rate is very important to choosing the right coefficients. If you change your program, adding a lot more floating point calculations, and your sample rate goes down by a factor of two, your time constant will go up by a factor of two unless you recalculate your filterterms.

As an example, consider using the 26.2 msec radio update as your control loop(generally a slow idea, but it does work). If you want a time constant of 0.75sec, the filter term would be:

對於絕大多數情況，設計濾波器通常以另一種方式進行。首先，取一個時間常數，然後用它來計算濾波器係數。所取時間常數應滿足系統的響應。如果陀螺儀平均每秒鐘漂移2°（可能是非常糟糕的估計了），時間常數可能需要小於一秒，以保證每秒鐘在任何方向上漂移度數不會超過兩度。但是時間常數越低，允許通過的水平加速度噪聲就越多。就像其他大多數控制面臨的狀況，這裏有一個平衡點，實踐是驗證真理的唯一標準。

記住對於選擇正確的濾波器係數，採樣頻率很重要。如果修改程序，加上更多的浮點運算，採樣頻率會下降一半，時間常量會上升一半，除非你重新計算濾波器係數。

舉個栗子，考慮使用26.2ms的速度更新控制週期（可能有點慢，但確實奏效）。如果想要得到0.75s的時間常數，濾波器係數應該是：

So,angle= (0.966)*(angle + gyro*0.0262) + (0.034)*(x_acc);.

The second filter coefficient, 0.034, is just (1 - 0.966).

It’s also worthwhile to think about what happens to the gyroscope bias in this filter. It definitely doesn’t cause the drifting problem, but it can still effect the angle calculation. Say, for example, we mistakenly chose the wrong offset and our gyroscope reports a rate of 5 º/sec rotation when it is stationary. It can be proven mathematically (I won’t here) that the effect of this on the angle estimate is just the offset rate multiplied by the time constant. So if we have a 0.75 sec time constant, this will give a constant angle offset of 3.75º.

Besides the fact that this is probably a worst-case scenario (the gyro should never be that far offset), a constant angle offset is much easier to deal with than a drifting angle offset. You could, for example, just rotate the accelerometer 3.75º in the opposite direction to accommodate for it.

濾波器裏的陀螺儀偏移的問題值得研究一下。它絕對不會導致漂移的問題，但仍然會影響角度的計算。舉個例子，我們疏忽地選擇了一個錯誤的補償，陀螺儀靜止時返回5°/s的旋轉速率。數學上可以證明（我就不證咯）這在角度估計的影響就是補償率乘以時間常量。所以如果我們取0.75s的時間常量，將會得到3.75°的角度補償常量。

除了這個非常糟糕的情況（陀螺儀永遠不需要那麼大的補償），角度補償常量遠比漂移角度補償容易處理。最後舉個栗子，反方向旋轉加速度計3.75°來消除這個誤差。

《The Balance Filter》互補濾波器--MIT著名牛文翻譯（上）http://blog.csdn.net/qq_32666555/article/details/54692956