《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