diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt
index 574f27002ac09b1b2ba16bcab215bb8146f59466..24b39cb71b74c95cdb7661d6151272f617268fb3 100644
--- a/src/lib/mathlib/CMakeLists.txt
+++ b/src/lib/mathlib/CMakeLists.txt
@@ -35,4 +35,5 @@ px4_add_library(mathlib
math/test/test.cpp
math/matrix_alg.cpp
math/filter/LowPassFilter2p.cpp
+ math/filter/LowPassFilter2pVector3f.cpp
)
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f595259df19e4733bf820cadd4326c14bb7dbd5
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp
@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "LowPassFilter2pVector3f.hpp"
+
+#include <px4_defines.h>
+
+#include <cmath>
+
+namespace math
+{
+
+void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+ _cutoff_freq = cutoff_freq;
+
+ // reset delay elements on filter change
+ _delay_element_1.zero();
+ _delay_element_2.zero();
+
+ if (_cutoff_freq <= 0.0f) {
+ // no filtering
+ _b0 = 0.0f;
+ _b1 = 0.0f;
+ _b2 = 0.0f;
+
+ _a1 = 0.0f;
+ _a2 = 0.0f;
+
+ return;
+ }
+
+ const float fr = sample_freq / _cutoff_freq;
+ const float ohm = tanf(M_PI_F / fr);
+ const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
+
+ _b0 = ohm * ohm / c;
+ _b1 = 2.0f * _b0;
+ _b2 = _b0;
+
+ _a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+ _a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+matrix::Vector3f LowPassFilter2pVector3f::reset(const matrix::Vector3f &sample)
+{
+ const matrix::Vector3f dval = sample / (_b0 + _b1 + _b2);
+
+ if (PX4_ISFINITE(dval(0)) && PX4_ISFINITE(dval(1)) && PX4_ISFINITE(dval(2))) {
+ _delay_element_1 = dval;
+ _delay_element_2 = dval;
+
+ } else {
+ _delay_element_1 = sample;
+ _delay_element_2 = sample;
+ }
+
+ return apply(sample);
+}
+
+} // namespace math
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..643cff73aa39c87e972431b50ab0dd8ff4ce644b
--- /dev/null
+++ b/src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/// @file LowPassFilter2pVector3f.hpp
+/// @brief A class to implement a second order low pass filter on a Vector3f
+/// Based on LowPassFilter2p.hpp by Leonard Hall <LeonardTHall@gmail.com>
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace math
+{
+class LowPassFilter2pVector3f
+{
+public:
+
+ LowPassFilter2pVector3f(float sample_freq, float cutoff_freq)
+ {
+ // set initial parameters
+ set_cutoff_frequency(sample_freq, cutoff_freq);
+ }
+
+ // Change filter parameters
+ void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
+ /**
+ * Add a new raw value to the filter
+ *
+ * @return retrieve the filtered result
+ */
+ inline matrix::Vector3f apply(const matrix::Vector3f &sample)
+ {
+ // do the filtering
+ const matrix::Vector3f delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
+ const matrix::Vector3f output{delay_element_0 *_b0 + _delay_element_1 *_b1 + _delay_element_2 * _b2};
+
+ _delay_element_2 = _delay_element_1;
+ _delay_element_1 = delay_element_0;
+
+ return output;
+ }
+
+ // Return the cutoff frequency
+ float get_cutoff_freq() const { return _cutoff_freq; }
+
+ // Reset the filter state to this value
+ matrix::Vector3f reset(const matrix::Vector3f &sample);
+
+private:
+
+ float _cutoff_freq{0.0f};
+
+ float _a1{0.0f};
+ float _a2{0.0f};
+
+ float _b0{0.0f};
+ float _b1{0.0f};
+ float _b2{0.0f};
+
+ matrix::Vector3f _delay_element_1{0.0f, 0.0f, 0.0f}; // buffered sample -1
+ matrix::Vector3f _delay_element_2{0.0f, 0.0f, 0.0f}; // buffered sample -2
+};
+
+} // namespace math