-
Notifications
You must be signed in to change notification settings - Fork 3
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
39c371f
commit 605df48
Showing
2 changed files
with
61 additions
and
17 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,8 +1,7 @@ | ||
/* | ||
* Copyright (C) 2022-2024 Dmitry Ponomarev <[email protected]> | ||
* 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 <[email protected]> | ||
*/ | ||
|
||
#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; | ||
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,19 +1,14 @@ | ||
/* | ||
* Copyright (C) 2022-2024 Dmitry Ponomarev <[email protected]> | ||
* 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 <[email protected]> | ||
*/ | ||
|
||
#ifndef SRC_COMMON_ALGORITHMS_H_ | ||
#define SRC_COMMON_ALGORITHMS_H_ | ||
|
||
#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_ |