Update KalmanSmoother.h

bareboat-necessities · Aug 31, 2024 · 14dd141 · 14dd141
1 parent 0351761
commit 14dd141
Showing 1 changed file with 14 additions and 14 deletions.
diff --git a/bbn_wave_freq_m5atomS3/KalmanSmoother.h b/bbn_wave_freq_m5atomS3/KalmanSmoother.h
@@ -36,39 +36,39 @@
    kalman_smoother_init(&kalman, 0.002, 30.0, 2000.0);
    kalman_smoother_set_initial(&kalman, 1.0);
    while(1) {
-     double measure = getSensorValue();
-     double measure_adjusted = kalman_smoother_update(&kalman, measure);
+     float measure = getSensorValue();
+     float measure_adjusted = kalman_smoother_update(&kalman, measure);
    }
    
 */
 
 typedef struct kalman_smoother_vars {
   /* Kalman filter variables */
-  double q; // process noise covariance
-  double r; // measurement uncertainty
-  double p; // estimation uncertainty
-  double k; // kalman gain
-  double x; // value
+  float q; // process noise covariance
+  float r; // measurement uncertainty
+  float p; // estimation uncertainty
+  float k; // kalman gain
+  float x; // value
 } KalmanSmootherVars;
 
-void kalman_smoother_init(KalmanSmootherVars* s, double process_noise, double sensor_noise, double estimated_error);
-void kalman_smoother_set_initial(KalmanSmootherVars* s, double intial_value);
-double kalman_smoother_update(KalmanSmootherVars* s, double measurement);
+void kalman_smoother_init(KalmanSmootherVars* s, float process_noise, float sensor_noise, float estimated_error);
+void kalman_smoother_set_initial(KalmanSmootherVars* s, float intial_value);
+float kalman_smoother_update(KalmanSmootherVars* s, float measurement);
 
-void kalman_smoother_init(KalmanSmootherVars* s, double process_noise, double sensor_noise, double estimated_error) {
+void kalman_smoother_init(KalmanSmootherVars* s, float process_noise, float sensor_noise, float estimated_error) {
   s->q = process_noise;
   s->r = sensor_noise;
   s->p = estimated_error;
 }
 
-void kalman_smoother_set_initial(KalmanSmootherVars* s, double intial_value) {
+void kalman_smoother_set_initial(KalmanSmootherVars* s, float intial_value) {
   s->x = intial_value;
 }
 
-double kalman_smoother_update(KalmanSmootherVars* s, double measurement) {
+float kalman_smoother_update(KalmanSmootherVars* s, float measurement) {
   // measurement update
   s->k = s->p / (s->p + s->r);
-  double current_estimate = s->x + s->k * (measurement - s->x);
+  float current_estimate = s->x + s->k * (measurement - s->x);
   s->p = (1.0 - s->k) * s->p + abs(s->x - current_estimate) * s->q;
   s->x = current_estimate;
   return s->x;