From acf707a5bbc3bf03c380ca64018b8986d6e9ad16 Mon Sep 17 00:00:00 2001
From: Mikhail Grushinskiy <mgrouch@users.noreply.github.com>
Date: Tue, 10 Sep 2024 09:45:12 -0400
Subject: [PATCH] Update AranovskiyFilter.h

---
 bbn_wave_freq_m5atomS3/AranovskiyFilter.h | 53 ++++++++++++-----------
 1 file changed, 27 insertions(+), 26 deletions(-)

diff --git a/bbn_wave_freq_m5atomS3/AranovskiyFilter.h b/bbn_wave_freq_m5atomS3/AranovskiyFilter.h
index 03c68d3..531edb8 100644
--- a/bbn_wave_freq_m5atomS3/AranovskiyFilter.h
+++ b/bbn_wave_freq_m5atomS3/AranovskiyFilter.h
@@ -16,13 +16,13 @@
 
   Usage example:
   
-  float omega_up = 1.0 * (2 * PI);  // upper frequency Hz * 2 * PI
-  float k_gain = 2.0;
-  float t_0 = 0.0;
-  float x1_0 = 0.0;
-  float theta_0 = - (omega_up * omega_up / 4.0);
-  float sigma_0 = theta_0;
-  float delta_t;  // time step sec
+  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;
@@ -37,7 +37,7 @@
     delay(4);
     
     // measure
-    float y = getAccelZfrom(imu) - 1.0;  // remove G acceration
+    double y = getAccelZfrom(imu) - 1.0;  // remove G acceration
 
     now = millis();
     delta_t = ((now - last_update) / 1000.0);
@@ -48,46 +48,47 @@
     // state.f contains estimated frequency
   }
 
+  Use double instead of float to avoid float overflows with higher Aranovskiy gain values
 */
 
 #define PI 3.1415926535897932384626433832795
 
 typedef struct aranovskiy_params {
-  float a = 1.0;
-  float b = a;
-  float k = 1.0;           // gain
+  double a = 1.0;
+  double b = a;
+  double k = 1.0;           // gain
 } AranovskiyParams;
 
 typedef struct aranovskiy_state {
-  float t = 0.0;           // time
-  float y;                 // signal measurement
-  float x1 = 0.0;
-  float theta = -0.25;
-  float sigma = -0.25;
-  float x1_dot;
-  float sigma_dot;
-  float omega;             // frequency * 2 * pi
-  float f;                 // frequency
+  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, float omega_up, float k_gain);
-void aranovskiy_init_state(AranovskiyState* s, float t_0, float x1_0, float theta_0, float sigma_0);
-void aranovskiy_update(AranovskiyParams* p, AranovskiyState* s, float y, float delta_t);
+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, float omega_up, float k_gain) {
+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, float t_0, float x1_0, float theta_0, float sigma_0) {
+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, float y, float delta_t) {
+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;