Define high and low pass complementary filters (#12)

Change-Id: I61bf8de3ff2018e32372cd4c28e02a0acd20a45d

WORKSPACE.bazel

@@ -286,7 +286,10 @@ cc_library(
     # https://github.com/mpusz/mp-units/issues/473
     #
     patch_args = ["-p1"],
-    patches = ["//third_party:mp-units-clang-priv-ctor.patch"],
+    patches = [
+        "//third_party:mp-units-clang-priv-ctor.patch",
+        "//third_party:mp-units-clang-type-identity.patch",
+    ],
     sha256 = "2c48171815909da836300d50a40e396412cb4881794b4cb5d13dcef858c46da1",
     strip_prefix = "mp-units-%s" % MP_UNITS_VERSION,
     url = "https://github.com/mpusz/mp-units/archive/%s.tar.gz" % MP_UNITS_VERSION,

complementary_filter/BUILD.bazel

@@ -0,0 +1,8 @@
+load("@rules_cc//cc:defs.bzl", "cc_library")
+
+cc_library(
+    name = "complementary_filter",
+    hdrs = ["complementary_filter.hpp"],
+    visibility = ["//:__subpackages__"],
+    deps = ["@mp_units"],
+)

complementary_filter/complementary_filter.hpp

@@ -0,0 +1,136 @@
+#pragma once
+
+#include <mp-units/systems/si/si.h>
+#include <numbers>
+
+// Low and high pass filters
+// https://gitlab.com/mechmotum/row_filter/-/blob/master/row_filter/complementary.py
+
+namespace complementary_filter {
+
+namespace mp = mp_units;
+namespace si = mp_units::si;
+
+template <
+    auto CutoffFrequency,
+    auto SamplePeriod,
+    class Rep = double,
+    mp::Quantity Q = mp::quantity<mp::si::radian, Rep>>
+struct lowpass
+{
+  using rep_type = Rep;
+
+  static constexpr auto w0 = mp::quantity<si::radian / si::second, rep_type>{
+      2 * std::numbers::pi_v<rep_type> * CutoffFrequency};
+  static constexpr auto dt = mp::quantity<si::second, rep_type>{SamplePeriod};
+
+  using value_type = Q;
+  using derivative_type = decltype(std::declval<value_type>() / dt);
+
+  struct signal
+  {
+    value_type value{};
+    derivative_type derivative{};
+  };
+
+  value_type prev_unfiltered{};
+  signal prev_filtered{};
+
+  constexpr auto operator()(value_type value) -> signal
+  {
+    static constexpr auto squared = [](auto x) { return x * x; };
+
+    static constexpr auto a = squared(w0);
+    static constexpr auto b = std::numbers::sqrt2_v<rep_type> * w0;
+
+    // Integrate the filter state equation using the midpoint Euler method with
+    // time step h
+    static constexpr auto h = dt;
+    static constexpr auto h2 = squared(h);
+    static constexpr auto denom = 4 * mp::one + 2 * h * b + h2 * a;
+
+    static constexpr auto A = (4 * mp::one + 2 * h * b - h2 * a) / denom;
+    static constexpr auto B = 4 * h / denom;
+    static constexpr auto C = -4 * h * a / denom;
+    static constexpr auto D = (4 * mp::one - 2 * h * b - h2 * a) / denom;
+    static constexpr auto E = 2 * h2 * a / denom;
+    static constexpr auto F = 4 * mp::one * h * a / denom;
+
+    const auto x0 = value;
+    const auto x1 = prev_unfiltered;
+    const auto [y1, yd1] = prev_filtered;
+
+    const auto y0 = A * y1 + B * yd1 + E * (x0 + x1) / 2;
+    const auto yd0 = C * y1 + D * yd1 + F * (x0 + x1) / 2;
+
+    prev_unfiltered = x0;
+    return prev_filtered = signal{y0, yd0};
+  }
+};
+
+template <
+    auto CutoffFrequency,
+    auto SamplePeriod,
+    class Rep = double,
+    mp::Quantity Q = mp::quantity<mp::si::radian, Rep>>
+struct highpass
+{
+  using rep_type = Rep;
+
+  static constexpr auto w0 = mp::quantity<si::radian / si::second, rep_type>{
+      2 * std::numbers::pi_v<rep_type> * CutoffFrequency};
+  static constexpr auto dt = mp::quantity<si::second, rep_type>{SamplePeriod};
+
+  using value_type = Q;
+
+  using state_type = std::tuple<
+      mp::quantity<mp::si::second, rep_type>,
+      mp::quantity<mp::square(mp::si::second), rep_type>>;
+
+  value_type prev_unfiltered{};
+  state_type prev_state{};
+
+  struct return_type
+  {
+    value_type value{};
+    state_type state{};
+  };
+
+  constexpr auto operator()(value_type value) -> return_type
+  {
+    static constexpr auto squared = [](auto x) { return x * x; };
+
+    static constexpr auto h = dt;
+
+    const auto xi = value;
+    const auto xim1 = prev_unfiltered;
+    const auto [z1im1, z2im1] = prev_state;
+
+    static constexpr auto a0 = std::numbers::sqrt2_v<rep_type> * h * w0;
+    static constexpr auto a1 = squared(h);
+    static constexpr auto a2 = squared(w0);
+    static constexpr auto a3 = a1 * a2;
+    static constexpr auto a4 = 2 * a0;
+    static constexpr auto a5 = a3 + a4 + 4 * mp::one;
+    static constexpr auto a6 = 1 / a5;
+    const auto a7 =
+        a1 * xi + a1 * xim1 - a3 * z2im1 + a4 * z2im1 + 4 * h * z1im1 +
+        4 * z2im1;
+    static constexpr auto a8 = a2 * h;
+
+    const auto z1i =
+        a6 *
+        (a5 * (-a0 * z1im1 - a8 * z2im1 + h * xi + h * xim1 + 2 * z1im1) -
+         a7 * a8) /
+        (a0 + 2 * mp::one);
+    const auto z2i = a6 * a7;
+    const auto yi = (z1i - z1im1) / h;
+
+    prev_unfiltered = xi;
+    prev_state = {z1i, z2i};
+
+    return return_type{yi, prev_state};
+  }
+};
+
+}  // namespace complementary_filter

complementary_filter/test/BUILD.bazel

@@ -0,0 +1,11 @@
+load("@rules_cc//cc:defs.bzl", "cc_test")
+
+cc_test(
+    name = "complementary_filter_test",
+    timeout = "short",
+    srcs = ["complementary_filter_test.cpp"],
+    deps = [
+        "//complementary_filter",
+        "@boost_ut",
+    ],
+)

complementary_filter/test/complementary_filter_test.cpp

@@ -0,0 +1,27 @@
+#include "complementary_filter/complementary_filter.hpp"
+
+#include <boost/ut.hpp>
+
+#include <mp-units/systems/si/si.h>
+
+auto main() -> int
+{
+  using ::boost::ut::expect;
+  using ::boost::ut::test;
+
+  test("complementary filter test") = [] {
+    using namespace mp_units::si::unit_symbols;
+
+    // NOLINTBEGIN(readability-magic-numbers)
+    {
+      auto lowpass = complementary_filter::lowpass<20 * Hz, 5 * ms>{};
+      [[maybe_unused]] const auto [y0, yd0] = lowpass(1.0 * rad);
+    }
+
+    {
+      auto highpass = complementary_filter::highpass<20 * Hz, 5 * ms>{};
+      [[maybe_unused]] const auto [y0, z0] = highpass(1.0 * rad);
+    }
+    // NOLINTEND(readability-magic-numbers)
+  };
+}

third_party/BUILD.bazel

@@ -1 +1,4 @@
-exports_files(["mp-units-clang-priv-ctor.patch"])
+exports_files([
+    "mp-units-clang-priv-ctor.patch",
+    "mp-units-clang-type-identity.patch",
+])

third_party/mp-units-clang-type-identity.patch

@@ -0,0 +1,12 @@
+diff --git a/src/core/include/mp-units/bits/external/type_list.h b/src/core/include/mp-units/bits/external/type_list.h
+index 5369ce18..80b66e06 100644
+--- a/src/core/include/mp-units/bits/external/type_list.h
++++ b/src/core/include/mp-units/bits/external/type_list.h
+@@ -24,6 +24,7 @@
+
+ #include <mp-units/bits/external/hacks.h>  // IWYU pragma: keep
+ #include <cstddef>
++#include <type_traits>
+ #include <utility>
+
+ MP_UNITS_DIAGNOSTIC_PUSH