|
|
|
|
@ -28,40 +28,19 @@ namespace modemm17 {
@@ -28,40 +28,19 @@ namespace modemm17 {
|
|
|
|
|
*/ |
|
|
|
|
class MODEMM17_API FreqDevEstimator |
|
|
|
|
{ |
|
|
|
|
using sample_filter_t = BaseIirFilter<3>; |
|
|
|
|
|
|
|
|
|
// IIR with Nyquist of 1/4.
|
|
|
|
|
static const std::array<float, 3> dc_b; |
|
|
|
|
static const std::array<float, 3> dc_a; |
|
|
|
|
|
|
|
|
|
static constexpr float MAX_DC_ERROR = 0.2; |
|
|
|
|
|
|
|
|
|
float min_est_ = 0.0; |
|
|
|
|
float max_est_ = 0.0; |
|
|
|
|
float min_cutoff_ = 0.0; |
|
|
|
|
float max_cutoff_ = 0.0; |
|
|
|
|
float min_var_ = 0.0; |
|
|
|
|
float max_var_ = 0.0; |
|
|
|
|
size_t min_count_ = 0; |
|
|
|
|
size_t max_count_ = 0; |
|
|
|
|
float deviation_ = 0.0; |
|
|
|
|
float offset_ = 0.0; |
|
|
|
|
float error_ = 0.0; |
|
|
|
|
float idev_ = 1.0; |
|
|
|
|
sample_filter_t dc_filter_{dc_b, dc_a}; |
|
|
|
|
|
|
|
|
|
public: |
|
|
|
|
using sample_filter_t = BaseIirFilter<3>; |
|
|
|
|
|
|
|
|
|
void reset() |
|
|
|
|
{ |
|
|
|
|
min_est_ = 0.0; |
|
|
|
|
max_est_ = 0.0; |
|
|
|
|
min_var_ = 0.0; |
|
|
|
|
max_var_ = 0.0; |
|
|
|
|
min_est_ = 0.0f; |
|
|
|
|
max_est_ = 0.0f; |
|
|
|
|
min_var_ = 0.0f; |
|
|
|
|
max_var_ = 0.0f; |
|
|
|
|
min_count_ = 0; |
|
|
|
|
max_count_ = 0; |
|
|
|
|
min_cutoff_ = 0.0; |
|
|
|
|
max_cutoff_ = 0.0; |
|
|
|
|
min_cutoff_ = 0.0f; |
|
|
|
|
max_cutoff_ = 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void sample(float sample) |
|
|
|
|
@ -70,8 +49,8 @@ public:
@@ -70,8 +49,8 @@ public:
|
|
|
|
|
{ |
|
|
|
|
min_count_ = 1; |
|
|
|
|
min_est_ = sample; |
|
|
|
|
min_var_ = 0.0; |
|
|
|
|
min_cutoff_ = min_est_ * 0.666666; |
|
|
|
|
min_var_ = 0.0f; |
|
|
|
|
min_cutoff_ = min_est_ * 0.666666f; |
|
|
|
|
} |
|
|
|
|
else if (sample < min_cutoff_) |
|
|
|
|
{ |
|
|
|
|
@ -80,12 +59,12 @@ public:
@@ -80,12 +59,12 @@ public:
|
|
|
|
|
float var = (min_est_ / min_count_) - sample; |
|
|
|
|
min_var_ += var * var; |
|
|
|
|
} |
|
|
|
|
else if (sample > 1.5 * max_est_) |
|
|
|
|
else if (sample > 1.5f * max_est_) |
|
|
|
|
{ |
|
|
|
|
max_count_ = 1; |
|
|
|
|
max_est_ = sample; |
|
|
|
|
max_var_ = 0.0; |
|
|
|
|
max_cutoff_ = max_est_ * 0.666666; |
|
|
|
|
max_var_ = 0.0f; |
|
|
|
|
max_cutoff_ = max_est_ * 0.666666f; |
|
|
|
|
} |
|
|
|
|
else if (sample > max_cutoff_) |
|
|
|
|
{ |
|
|
|
|
@ -104,27 +83,54 @@ public:
@@ -104,27 +83,54 @@ public:
|
|
|
|
|
*/ |
|
|
|
|
void update() |
|
|
|
|
{ |
|
|
|
|
if (max_count_ < 2 || min_count_ < 2) return; |
|
|
|
|
if (max_count_ < 2 || min_count_ < 2) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float max_ = max_est_ / max_count_; |
|
|
|
|
float min_ = min_est_ / min_count_; |
|
|
|
|
deviation_ = (max_ - min_) / 6.0; |
|
|
|
|
offset_ = dc_filter_(std::max(std::min(max_ + min_, deviation_ * MAX_DC_ERROR), deviation_ * -MAX_DC_ERROR)); |
|
|
|
|
error_ = (std::sqrt(max_var_ / (max_count_ - 1)) + std::sqrt(min_var_ / (min_count_ - 1))) * 0.5; |
|
|
|
|
if (deviation_ > 0) idev_ = 1.0 / deviation_; |
|
|
|
|
min_cutoff_ = offset_ - deviation_ * 2; |
|
|
|
|
|
|
|
|
|
if (deviation_ > 0) { |
|
|
|
|
idev_ = 1.0 / deviation_; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
min_cutoff_ = offset_ - deviation_ * 2; |
|
|
|
|
max_cutoff_ = offset_ + deviation_ * 2; |
|
|
|
|
max_est_ = max_; |
|
|
|
|
min_est_ = min_; |
|
|
|
|
max_count_ = 1; |
|
|
|
|
min_count_ = 1; |
|
|
|
|
max_var_ = 0.0; |
|
|
|
|
min_var_ = 0.0; |
|
|
|
|
max_var_ = 0.0f; |
|
|
|
|
min_var_ = 0.0f; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float deviation() const { return deviation_; } |
|
|
|
|
float offset() const { return offset_; } |
|
|
|
|
float error() const { return error_; } |
|
|
|
|
float idev() const { return idev_; } |
|
|
|
|
|
|
|
|
|
private: |
|
|
|
|
// IIR with Nyquist of 1/4.
|
|
|
|
|
static const std::array<float, 3> dc_b; |
|
|
|
|
static const std::array<float, 3> dc_a; |
|
|
|
|
static const float MAX_DC_ERROR; |
|
|
|
|
|
|
|
|
|
float min_est_ = 0.0f; |
|
|
|
|
float max_est_ = 0.0f; |
|
|
|
|
float min_cutoff_ = 0.0f; |
|
|
|
|
float max_cutoff_ = 0.0f; |
|
|
|
|
float min_var_ = 0.0f; |
|
|
|
|
float max_var_ = 0.0f; |
|
|
|
|
size_t min_count_ = 0; |
|
|
|
|
size_t max_count_ = 0; |
|
|
|
|
float deviation_ = 0.0f; |
|
|
|
|
float offset_ = 0.0f; |
|
|
|
|
float error_ = 0.0f; |
|
|
|
|
float idev_ = 1.0f; |
|
|
|
|
sample_filter_t dc_filter_{dc_b, dc_a}; |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
} // modemm17
|
|
|
|
|
|