1 files changed, 55 insertions, 0 deletions
diff --git a/src/AltEst/altitude.h b/src/AltEst/altitude.h
new file mode 100644
index 0000000..1ca6cb0
--- /dev/null
+++ b/src/AltEst/altitude.h
@@ -0,0 +1,55 @@
+/*
+    altitude.h: Altitude estimation via barometer/accelerometer fusion
+*/
+
+# pragma once
+
+#include "filters.h"
+#include "algebra.h"
+#include "pico/time.h"
+#include "pico/types.h"
+
+class AltitudeEstimator {
+
+  private:
+    // required parameters for the filters used for the estimations
+    // sensor's standard deviations
+    float sigmaAccel;
+    float sigmaGyro;
+    float sigmaBaro;
+    // Acceleration markov chain model state transition constant
+    float ca;
+    // Zero-velocity update acceleration threshold
+    float accelThreshold;
+    // gravity
+    float g = 9.81;
+    // For computing the sampling period
+    absolute_time_t prevTime = get_absolute_time();
+    uint32_t previousTime = to_us_since_boot(prevTime);
+    // required filters for altitude and vertical velocity estimation
+    KalmanFilter kalman;
+    ComplementaryFilter complementary;
+    // Estimated past vertical acceleration
+    float pastVerticalAccel = 0;
+    float pastVerticalVelocity = 0;
+    float pastAltitude = 0;
+    float pastGyro[3] = {0, 0, 0};
+    float pastAccel[3] = {0, 0, 0};
+    // estimated altitude and vertical velocity
+    float estimatedAltitude = 0;
+    float estimatedVelocity = 0;
+
+  public:
+
+    AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro,
+                      float ca, float accelThreshold);
+
+    void estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp);
+
+    float getAltitude();
+
+    float getVerticalVelocity();
+
+    float getVerticalAcceleration();
+
+}; // class AltitudeEstimator