From 605df48dfa716af843996b0a7348398d882e5d20 Mon Sep 17 00:00:00 2001
From: PonomarevDA <ponomarevda96@gmail.com>
Date: Sat, 12 Oct 2024 13:35:11 +0300
Subject: [PATCH] add adaptive alpha filter

---
 Src/common/algorithms.cpp | 40 ++++++++++++++++++++++++++++++++++-----
 Src/common/algorithms.hpp | 38 +++++++++++++++++++++++++------------
 2 files changed, 61 insertions(+), 17 deletions(-)

diff --git a/Src/common/algorithms.cpp b/Src/common/algorithms.cpp
index 2c0e523..266e84c 100644
--- a/Src/common/algorithms.cpp
+++ b/Src/common/algorithms.cpp
@@ -1,8 +1,7 @@
-/*
- * Copyright (C) 2022-2024 Dmitry Ponomarev <ponomarevda96@gmail.com>
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
  */
 
 #include "algorithms.hpp"
@@ -64,3 +63,34 @@ void movingAverage(float* prev_avg, float crnt_val, uint16_t size) {
         *prev_avg = (*prev_avg * (size - 1) + crnt_val) / size;
     }
 }
+
+
+AdaptiveAlphaFilter::AdaptiveAlphaFilter(float small_delta, float large_delta,
+                                         float smooth_alpha, float fast_alpha) :
+    SMALL_DELTA(small_delta),
+    LARGE_DELTA(large_delta),
+    SMOOTH_ALPHA(smooth_alpha),
+    FAST_ALPHA(fast_alpha) {
+}
+
+float AdaptiveAlphaFilter::update(float new_value) {
+    float delta = new_value - previous_filtered_value;
+    float alpha = linearly_interpolate_alpha(delta);
+    previous_filtered_value += alpha * delta;
+    return previous_filtered_value;
+}
+
+float AdaptiveAlphaFilter::linearly_interpolate_alpha(float delta) const {
+    auto abs_delta = fabs(delta);
+    float alpha;
+    if (abs_delta <= SMALL_DELTA) {
+        alpha = SMOOTH_ALPHA;
+    } else if (abs_delta >= LARGE_DELTA) {
+        alpha = FAST_ALPHA;
+    } else {
+        float fraction = (abs_delta - SMALL_DELTA) / (LARGE_DELTA - SMALL_DELTA);
+        alpha = SMOOTH_ALPHA + fraction * (FAST_ALPHA - SMOOTH_ALPHA);
+    }
+
+    return alpha;
+}
diff --git a/Src/common/algorithms.hpp b/Src/common/algorithms.hpp
index 683b5f0..d041c38 100644
--- a/Src/common/algorithms.hpp
+++ b/Src/common/algorithms.hpp
@@ -1,8 +1,7 @@
-/*
- * Copyright (C) 2022-2024 Dmitry Ponomarev <ponomarevda96@gmail.com>
- * This Source Code Form is subject to the terms of the Mozilla Public
- * License, v. 2.0. If a copy of the MPL was not distributed with this
- * file, You can obtain one at https://mozilla.org/MPL/2.0/.
+/**
+ * This program is free software under the GNU General Public License v3.
+ * See <https://www.gnu.org/licenses/> for details.
+ * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
  */
 
 #ifndef SRC_COMMON_ALGORITHMS_H_
@@ -10,10 +9,6 @@
 
 #include <stdint.h>
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 typedef uint16_t PwmDurationUs;
 
 /**
@@ -63,8 +58,27 @@ float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max);
 
 void movingAverage(float* prev_avg, float crnt_val, uint16_t size);
 
-#ifdef __cplusplus
-}
-#endif
+/**
+ * @brief The Adaptive Alpha Filter is a variation of the exponential smoothing filter,
+ * where the smoothing factor α is adjusted dynamically based on the magnitude of changes
+ * between consecutive inputs.
+ */
+class AdaptiveAlphaFilter {
+public:
+    AdaptiveAlphaFilter(float small_delta = 0.0f, float large_delta = 100.0f,
+                        float smooth_alpha = 0.10f, float fast_alpha = 0.90f);
+
+    float update(float new_value);
+
+private:
+    float linearly_interpolate_alpha(float delta) const;
+
+    const float SMALL_DELTA;
+    const float LARGE_DELTA;
+    const float SMOOTH_ALPHA;
+    const float FAST_ALPHA;
+
+    float previous_filtered_value{0.0f};
+};
 
 #endif  // SRC_COMMON_ALGORITHMS_H_