From 5adc0a46b54b45538dd34f336e2693773e213dcd Mon Sep 17 00:00:00 2001
From: Mikhail Grushinskiy <mgrouch@users.noreply.github.com>
Date: Tue, 27 Aug 2024 10:32:06 -0400
Subject: [PATCH] Update AranovskiyFilter.h

---
 bbn_wave_freq_m5atomS3/AranovskiyFilter.h | 99 +++++++++++++++++++++++
 1 file changed, 99 insertions(+)

diff --git a/bbn_wave_freq_m5atomS3/AranovskiyFilter.h b/bbn_wave_freq_m5atomS3/AranovskiyFilter.h
index 8b13789..a04a1db 100644
--- a/bbn_wave_freq_m5atomS3/AranovskiyFilter.h
+++ b/bbn_wave_freq_m5atomS3/AranovskiyFilter.h
@@ -1 +1,100 @@
+#ifndef AranovskiyFilter_h
+#define AranovskiyFilter_h
+
+/*
+
+  Aranovskiy frequency estimator which is a simple on-line filter.
+
+  Ref:
+
+  Alexey A. Bobtsov, Nikolay A. Nikolaev, Olga V. Slita, Alexander S. Borgul, Stanislav V. Aranovskiy
+
+  The New Algorithm of Sinusoidal Signal Frequency Estimation.
+
+  11th IFAC International Workshop on Adaptation and Learning in Control and Signal Processing July 3-5, 2013. Caen, France
+
+  Usage example:
+  
+  double omega_up = 1.0 * (2 * PI);  // upper frequency Hz * 2 * PI
+  double k_gain = 2.0;
+  double t_0 = 0.0;
+  double x1_0 = 0.0;
+  double theta_0 = - (omega_up * omega_up / 4.0);
+  double sigma_0 = theta_0;
+  double delta_t;  // time step sec
+
+  AranovskiyParams params;
+  AranovskiyState  state;
+
+  aranovskiy_default_params(&params, omega_up, k_gain);
+  aranovskiy_init_state(&state, t_0, x1_0, theta_0, sigma_0);
+
+  unsigned long now = 0UL, last_update = 0UL;
+
+  last_update = millis();
+  while(1) {
+    delay(4);
+    
+    // measure
+    double y = getAccelZfrom(imu) - 1.0;  // remove G acceration
+
+    now = millis();
+    delta_t = ((now - last_update) / 1000.0);
+    last_update = now;
+
+    aranovskiy_update(&params, &state, y, delta_t);
+
+    // state.f contains estimated frequency
+  }
+
+*/
+
+typedef struct aranovskiy_params {
+  double a = 1.0;
+  double b = a;
+  double k = 1.0;           // gain
+} AranovskiyParams;
+
+typedef struct aranovskiy_state {
+  double t = 0.0;           // time
+  double y;                 // signal measurement
+  double x1 = 0.0;
+  double theta = -0.25;
+  double sigma = -0.25;
+  double x1_dot;
+  double sigma_dot;
+  double omega;             // frequency * 2 * pi
+  double f;                 // frequency
+} AranovskiyState;
+
+void aranovskiy_default_params(AranovskiyParams* p, double omega_up, double k_gain);
+void aranovskiy_init_state(AranovskiyState* s, double t_0, double x1_0, double theta_0, double sigma_0);
+void aranovskiy_update(AranovskiyParams* p, AranovskiyState* s, double y, double delta_t);
+
+void aranovskiy_default_params(AranovskiyParams* p, double omega_up, double k_gain) {
+  p->a = omega_up;
+  p->b = p->a;
+  p->k = k_gain;
+}
+
+void aranovskiy_init_state(AranovskiyState* s, double t_0, double x1_0, double theta_0, double sigma_0) {
+  s->t = t_0;
+  s->x1 = x1_0;
+  s->theta = theta_0;
+  s->sigma = sigma_0;
+}
+
+void aranovskiy_update(AranovskiyParams* p, AranovskiyState* s, double y, double delta_t) {
+  s->x1_dot = - p->a * s->x1 + p->b * y;
+  s->sigma_dot = - p->k * s->x1 * s->x1 * s->theta - p->k * p->a * s->x1 * s->x1_dot - p->k * p->b * s->x1_dot * y;
+  s->theta = s->sigma + p->k * p->b * s->x1 * y;
+  s->omega = sqrt(abs(s->theta));
+  s->f = s->omega / (2.0 * PI);
+  // step
+  s->x1 = s->x1 + s->x1_dot * delta_t;
+  s->sigma = s->sigma + s->sigma_dot * delta_t;
+  s->t = s->t + delta_t;
+}
+
+#endif