Arduino Code – Complementary Filter

My current Arduino project is a self-balancing robot, which requires repeatedly reading angle and acceleration values from an MPU-6050 gyro.

These values can be quite ‘noisy’ – fluctuating considerably when vibration is added into the system – so I found myself in need of some simple code that helps smooth out the values…

Here’s the class I wrote – FilteredFloat, a simple Complementary Filter:

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

class FilteredFloat
{
public:
  FilteredFloat(float smoothing = 0.7) {
    m_inited = false;
    m_d = smoothing;
    m_value = m_oldValue = m_raw = 0.0;
  }

  void set(float value) {
    m_raw = value;
    
    if (!m_inited) {
      m_inited = true;
      m_oldValue = m_value = value;
      return;
    }
      
    m_value = m_d * m_oldValue + (1.0 - m_d) * value;
    m_oldValue = m_value;
  }

  float get() const {
    return m_value;
  }

  float getRaw() const {
    return m_raw;
  }

private:
  bool m_inited;
  float m_d;
  float m_value, m_oldValue, m_raw;
};

(See my earlier article on how to put this code into its own header file, to make it even easier to use in your projects.)

After creating an instance of the object, just repeatedly call set(...) which each new reading value. The code will calculate a ‘running current value’ by combining the previous ‘set’ value and the new one, which you can retrieve using get().

smooth_value = F * old_value + (1.0 - F) * new_value

To apply more averaging try increasing the ‘smoothing’ value in the constructor (up to a value nearing 1.0). A smoothing of zero will result in no smoothing at all.

Example:

1
2
3
4
5
6
7
8
9

// Change smoothing value as required.
FilteredFloat myFloat(0.95);

while (busy) {
  myFloat.set(getNewValue());

  float smoothed = myFloat.get();
  // todo - Use 'smoothed'...
}