diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp
new file mode 100644
index 0000000000..6d1ffb261e
--- /dev/null
+++ b/libraries/Filter/LowPassFilter.cpp
@@ -0,0 +1,94 @@
+//
+/// @file LowPassFilter.cpp
+/// @brief  A class to implement a low pass filter without losing precision even for int types
+///         the downside being that it's a little slower as it internally uses a float
+///         and it consumes an extra 4 bytes of memory to hold the constant gain
+
+
+#include "LowPassFilter.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////
+// DigitalLPF
+////////////////////////////////////////////////////////////////////////////////////////////
+
+template <class T>
+DigitalLPF<T>::DigitalLPF() {
+  // built in initialization
+  _output = T();
+}
+
+// add a new raw value to the filter, retrieve the filtered result
+template <class T>
+T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
+    if (cutoff_freq <= 0.0f || dt <= 0.0f) {
+        _output = sample;
+        return _output;
+    }
+
+    float rc = 1.0f/(M_2PI_F*cutoff_freq);
+    float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
+    _output += (sample - _output) * alpha;
+    return _output;
+}
+
+// get latest filtered value from filter (equal to the value returned by latest call to apply method)
+template <class T>
+const T &DigitalLPF<T>::get() const {
+    return _output;
+}
+
+template <class T>
+void DigitalLPF<T>::reset(T value) { 
+    _output = value; 
+}
+    
+////////////////////////////////////////////////////////////////////////////////////////////
+// LowPassFilter
+////////////////////////////////////////////////////////////////////////////////////////////
+template <class T>
+LowPassFilter<T>::LowPassFilter() : _cutoff_freq(0.0f) { 
+  
+}
+// constructor
+template <class T>
+LowPassFilter<T>::LowPassFilter(float cutoff_freq) : _cutoff_freq(cutoff_freq) { 
+  
+}
+
+// change parameters
+template <class T>
+void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
+    _cutoff_freq = cutoff_freq;
+}
+
+// return the cutoff frequency
+template <class T>
+float LowPassFilter<T>::get_cutoff_freq(void) const {
+    return _cutoff_freq;
+}
+
+template <class T>
+T LowPassFilter<T>::apply(T sample, float dt) {
+    return _filter.apply(sample, _cutoff_freq, dt);
+}
+
+template <class T>
+const T &LowPassFilter<T>::get() const {
+    return _filter.get();
+}
+
+template <class T>
+void LowPassFilter<T>::reset(T value) {
+    _filter.reset(value);
+}
+
+/* 
+ * Make an instances
+ * Otherwise we have to move the constructor implementations to the header file :P
+ */
+template class LowPassFilter<int>;
+template class LowPassFilter<long>;
+template class LowPassFilter<float>;
+template class LowPassFilter<double>;
+template class LowPassFilter<Vector2f>;
+template class LowPassFilter<Vector3f>;
\ No newline at end of file
diff --git a/libraries/Filter/LowPassFilter.h b/libraries/Filter/LowPassFilter.h
index c551c18537..5ac478ca1b 100644
--- a/libraries/Filter/LowPassFilter.h
+++ b/libraries/Filter/LowPassFilter.h
@@ -27,156 +27,67 @@
 #include "FilterClass.h"
 
 // DigitalLPF implements the filter math
-class DigitalLPF
-{
+template <class T>
+class DigitalLPF {
 public:
-    // constructor
-    DigitalLPF() :
-    _output(0.0f) {}
-
     struct lpf_params {
         float cutoff_freq;
         float sample_freq;
         float alpha;
-    };
+    };  
 
+    DigitalLPF();
     // add a new raw value to the filter, retrieve the filtered result
-    float apply(float sample, float cutoff_freq, float dt) {
-        if (cutoff_freq <= 0.0f || dt <= 0.0f) {
-            _output = sample;
-            return _output;
-        }
-
-        float rc = 1.0f/(M_2PI_F*cutoff_freq);
-        float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
-        _output += (sample - _output) * alpha;
-        return _output;
-    }
-
+    T apply(const T &sample, float cutoff_freq, float dt);
     // get latest filtered value from filter (equal to the value returned by latest call to apply method)
-    float get() const {
-        return _output;
-    }
-
-    void reset(float value) { _output = value; }
+    const T &get() const;
+    void reset(T value);
 
 private:
-    float _output;
+    T _output;
 };
 
 // LPF base class
-class LowPassFilter
-{
+template <class T>
+class LowPassFilter {
 public:
-    LowPassFilter() :
-    _cutoff_freq(0.0f) { }
-    // constructor
-    LowPassFilter(float cutoff_freq) :
-    _cutoff_freq(cutoff_freq) { }
+    LowPassFilter();
+    LowPassFilter(float cutoff_freq);
 
     // change parameters
-    void set_cutoff_frequency(float cutoff_freq) {
-        _cutoff_freq = cutoff_freq;
-    }
-
+    void set_cutoff_frequency(float cutoff_freq);
     // return the cutoff frequency
-    float get_cutoff_freq(void) const {
-        return _cutoff_freq;
-    }
-
+    float get_cutoff_freq(void) const;
+    T apply(T sample, float dt);
+    const T &get() const;
+    void reset(T value);
+    
 protected:
     float _cutoff_freq;
-};
-
-// LPF for a single float
-class LowPassFilterFloat : public LowPassFilter
-{
-public:
-    LowPassFilterFloat() :
-    LowPassFilter() {}
-
-    LowPassFilterFloat(float cutoff_freq):
-    LowPassFilter(cutoff_freq) {}
-
-    float apply(float sample, float dt) {
-        return _filter.apply(sample, _cutoff_freq, dt);
-    }
-
-    float get() const {
-        return _filter.get();
-    }
-
-    void reset(float value) {
-        _filter.reset(value);
-    }
-private:
-    DigitalLPF _filter;
-};
-
-// LPF for a 2D vector
-class LowPassFilterVector2f : public LowPassFilter
-{
-public:
-    LowPassFilterVector2f() :
-    LowPassFilter() {}
-
-    LowPassFilterVector2f(float cutoff_freq) :
-    LowPassFilter(cutoff_freq) {}
-
-    Vector2f apply(const Vector2f &sample, float dt) {
-        Vector2f ret;
-        ret.x = _filter_x.apply(sample.x, _cutoff_freq, dt);
-        ret.y = _filter_y.apply(sample.y, _cutoff_freq, dt);
-        return ret;
-    }
-
-    void reset(const Vector2f& value) {
-        _filter_x.reset(value.x);
-        _filter_y.reset(value.y);
-    }
 
 private:
-    DigitalLPF _filter_x;
-    DigitalLPF _filter_y;
+    DigitalLPF<T> _filter;
 };
 
-// LPF for 3D vector
-class LowPassFilterVector3f : public LowPassFilter
-{
-public:
-    LowPassFilterVector3f() :
-    LowPassFilter() {}
+// Uncomment this, if you decide to remove the instantiations in the implementation file
+/*
+template <class T>
+LowPassFilter<T>::LowPassFilter() : _cutoff_freq(0.0f) { 
+  
+}
+// constructor
+template <class T>
+LowPassFilter<T>::LowPassFilter(float cutoff_freq) : _cutoff_freq(cutoff_freq) { 
+  
+}
+*/
 
-    LowPassFilterVector3f(float cutoff_freq) :
-    LowPassFilter(cutoff_freq) {}
+// typedefs for compatibility
+typedef LowPassFilter<int>      LowPassFilterInt;
+typedef LowPassFilter<long>     LowPassFilterLong;
+typedef LowPassFilter<float>    LowPassFilterFloat;
+typedef LowPassFilter<double>   LowPassFilterDouble;
+typedef LowPassFilter<Vector2f> LowPassFilterVector2f;
+typedef LowPassFilter<Vector3f> LowPassFilterVector3f;
 
-    Vector3f apply(const Vector3f &sample, float dt) {
-        Vector3f ret;
-        ret.x = _filter_x.apply(sample.x, _cutoff_freq, dt);
-        ret.y = _filter_y.apply(sample.y, _cutoff_freq, dt);
-        ret.z = _filter_z.apply(sample.z, _cutoff_freq, dt);
-        return ret;
-    }
-
-    // get latest filtered value from filter (equal to the value returned by latest call to apply method)
-    Vector3f get() const {
-        Vector3f ret;
-        ret.x = _filter_x.get();
-        ret.y = _filter_y.get();
-        ret.z = _filter_z.get();
-        return ret;
-    }
-
-    void reset(const Vector3f& value) {
-        _filter_x.reset(value.x);
-        _filter_y.reset(value.y);
-        _filter_z.reset(value.z);
-    }
-
-private:
-    DigitalLPF _filter_x;
-    DigitalLPF _filter_y;
-    DigitalLPF _filter_z;
-};
-
-#endif // __LOW_PASS_FILTER_H__
+#endif // __LOW_PASS_FILTER_H__
\ No newline at end of file
diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp
index dbd0972dc3..dd963d5644 100644
--- a/libraries/Filter/LowPassFilter2p.cpp
+++ b/libraries/Filter/LowPassFilter2p.cpp
@@ -1,39 +1,24 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/*
-   This program is free software: you can redistribute it and/or modify
-   it under the terms of the GNU General Public License as published by
-   the Free Software Foundation, either version 3 of the License, or
-   (at your option) any later version.
-
-   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
-
-   You should have received a copy of the GNU General Public License
-   along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-/// @file	LowPassFilter.cpp
-/// @brief	A class to implement a second order low pass filter 
-/// Author: Leonard Hall <LeonardTHall@gmail.com>
-
-#include <inttypes.h>
-#include <AP_Math/AP_Math.h>
 #include "LowPassFilter2p.h"
 
-float DigitalBiquadFilter::apply(float sample, const struct biquad_params &params)
-{
+
+////////////////////////////////////////////////////////////////////////////////////////////
+// DigitalBiquadFilter
+////////////////////////////////////////////////////////////////////////////////////////////
+
+template <class T>
+DigitalBiquadFilter<T>::DigitalBiquadFilter() {
+  _delay_element_1 = T();
+  _delay_element_2 = T();
+}
+
+template <class T>
+T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &params) {
     if(is_zero(params.cutoff_freq) || is_zero(params.sample_freq)) {
         return sample;
     }
 
-    float delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
-    if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        delay_element_0 = sample;
-    }
-    float output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
+    T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
+    T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
 
     _delay_element_2 = _delay_element_1;
     _delay_element_1 = delay_element_0;
@@ -41,8 +26,13 @@ float DigitalBiquadFilter::apply(float sample, const struct biquad_params &param
     return output;
 }
 
-void DigitalBiquadFilter::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret)
-{
+template <class T>
+void DigitalBiquadFilter<T>::reset() { 
+    _delay_element_1 = _delay_element_2 = T();
+}
+
+template <class T>
+void DigitalBiquadFilter<T>::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret) {
     ret.cutoff_freq = cutoff_freq;
     ret.sample_freq = sample_freq;
 
@@ -56,3 +46,53 @@ void DigitalBiquadFilter::compute_params(float sample_freq, float cutoff_freq, b
     ret.a1 = 2.0f*(ohm*ohm-1.0f)/c;
     ret.a2 = (1.0f-2.0f*cosf(PI/4.0f)*ohm+ohm*ohm)/c;
 }
+
+
+////////////////////////////////////////////////////////////////////////////////////////////
+// LowPassFilter2p
+////////////////////////////////////////////////////////////////////////////////////////////
+
+template <class T>
+LowPassFilter2p<T>::LowPassFilter2p() { 
+    memset(&_params, 0, sizeof(_params) ); 
+}
+
+// constructor
+template <class T>
+LowPassFilter2p<T>::LowPassFilter2p(float sample_freq, float cutoff_freq) {
+    // set initial parameters
+    set_cutoff_frequency(sample_freq, cutoff_freq);
+}
+
+// change parameters
+template <class T>
+void LowPassFilter2p<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) {
+    DigitalBiquadFilter<T>::compute_params(sample_freq, cutoff_freq, _params);
+}
+
+// return the cutoff frequency
+template <class T>
+float LowPassFilter2p<T>::get_cutoff_freq(void) const {
+    return _params.cutoff_freq;
+}
+
+template <class T>
+float LowPassFilter2p<T>::get_sample_freq(void) const {
+    return _params.sample_freq;
+}
+
+template <class T>
+T LowPassFilter2p<T>::apply(const T &sample) {
+    return _filter.apply(sample, _params);
+}
+
+/* 
+ * Make an instances
+ * Otherwise we have to move the constructor implementations to the header file :P
+ */
+template class LowPassFilter2p<int>;
+template class LowPassFilter2p<long>;
+template class LowPassFilter2p<float>;
+template class LowPassFilter2p<double>;
+template class LowPassFilter2p<Vector2f>;
+template class LowPassFilter2p<Vector3f>;
diff --git a/libraries/Filter/LowPassFilter2p.h b/libraries/Filter/LowPassFilter2p.h
index 05f797bf1b..bab01c599e 100644
--- a/libraries/Filter/LowPassFilter2p.h
+++ b/libraries/Filter/LowPassFilter2p.h
@@ -18,17 +18,17 @@
 #ifndef LOWPASSFILTER2P_H
 #define LOWPASSFILTER2P_H
 
+#include <AP_Math/AP_Math.h>
+#include <math.h>
+#include <inttypes.h>
+
+
 /// @file   LowPassFilter2p.h
 /// @brief  A class to implement a second order low pass filter
-/// Author: Leonard Hall <LeonardTHall@gmail.com>
-
-class DigitalBiquadFilter
-{
+/// @authors: Leonard Hall <LeonardTHall@gmail.com>, template implmentation: Daniel Frenzel <dgdanielf@gmail.com>
+template <class T>
+class DigitalBiquadFilter {
 public:
-    DigitalBiquadFilter() :
-    _delay_element_1(0.0f),
-    _delay_element_2(0.0f){}
-
     struct biquad_params {
         float cutoff_freq;
         float sample_freq;
@@ -38,84 +38,59 @@ public:
         float b1;
         float b2;
     };
+  
+    DigitalBiquadFilter();
 
-    float apply(float sample, const struct biquad_params &params);
-
-    void reset() { _delay_element_1 = _delay_element_2 = 0.0f; }
-
+    T apply(const T &sample, const struct biquad_params &params);
+    void reset();
     static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret);
-
+    
 private:
-    float _delay_element_1;
-    float _delay_element_2;
+    T _delay_element_1;
+    T _delay_element_2;
 };
 
-class LowPassFilter2p
-{
+template <class T>
+class LowPassFilter2p {
 public:
-    LowPassFilter2p() { memset(&_params, 0, sizeof(_params)); }
+    LowPassFilter2p();
     // constructor
-    LowPassFilter2p(float sample_freq, float cutoff_freq) {
-        // set initial parameters
-        set_cutoff_frequency(sample_freq, cutoff_freq);
-    }
-
+    LowPassFilter2p(float sample_freq, float cutoff_freq);
     // change parameters
-    void set_cutoff_frequency(float sample_freq, float cutoff_freq) {
-        DigitalBiquadFilter::compute_params(sample_freq, cutoff_freq, _params);
-    }
-
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
     // return the cutoff frequency
-    float get_cutoff_freq(void) const {
-        return _params.cutoff_freq;
-    }
-
-    float get_sample_freq(void) const {
-        return _params.sample_freq;
-    }
+    float get_cutoff_freq(void) const;
+    float get_sample_freq(void) const;
+    T apply(const T &sample);
 
 protected:
-    struct DigitalBiquadFilter::biquad_params _params;
-};
-
-class LowPassFilter2pfloat : public LowPassFilter2p
-{
-public:
-    LowPassFilter2pfloat() :
-    LowPassFilter2p() {}
-
-    LowPassFilter2pfloat(float sample_freq, float cutoff_freq):
-    LowPassFilter2p(sample_freq,cutoff_freq) {}
-
-    float apply(float sample) {
-        return _filter.apply(sample, _params);
-    }
+    struct DigitalBiquadFilter<T>::biquad_params _params;
+    
 private:
-    DigitalBiquadFilter _filter;
+    DigitalBiquadFilter<T> _filter;
 };
 
-class LowPassFilter2pVector3f : public LowPassFilter2p
-{
-public:
-    LowPassFilter2pVector3f() :
-    LowPassFilter2p() {}
+// Uncomment this, if you decide to remove the instantiations in the implementation file
+/*
+template <class T>
+LowPassFilter2p<T>::LowPassFilter2p() { 
+    memset(&_params, 0, sizeof(_params) ); 
+}
 
-    LowPassFilter2pVector3f(float sample_freq, float cutoff_freq) :
-    LowPassFilter2p(sample_freq,cutoff_freq) {}
+// constructor
+template <class T>
+LowPassFilter2p<T>::LowPassFilter2p(float sample_freq, float cutoff_freq) {
+    // set initial parameters
+    set_cutoff_frequency(sample_freq, cutoff_freq);
+}
+*/
 
-    Vector3f apply(const Vector3f &sample) {
-        Vector3f ret;
-        ret.x = _filter_x.apply(sample.x, _params);
-        ret.y = _filter_y.apply(sample.y, _params);
-        ret.z = _filter_z.apply(sample.z, _params);
-        return ret;
-    }
-
-private:
-    DigitalBiquadFilter _filter_x;
-    DigitalBiquadFilter _filter_y;
-    DigitalBiquadFilter _filter_z;
-};
+typedef LowPassFilter2p<int>      LowPassFilter2pInt;
+typedef LowPassFilter2p<long>     LowPassFilter2pLong;
+typedef LowPassFilter2p<float>    LowPassFilter2pFloat;
+typedef LowPassFilter2p<double>   LowPassFilter2pDouble;
+typedef LowPassFilter2p<Vector2f> LowPassFilter2pVector2f;
+typedef LowPassFilter2p<Vector3f> LowPassFilter2pVector3f;
 
 
-#endif // LOWPASSFILTER2P_H
+#endif // LOWPASSFILTER2P_H
\ No newline at end of file