Kf onboarding (#58)

* Co-authored-by: SZhOU-c <SZhOU-c@users.noreply.github.com>

* ]

Co-authored-by: SZhOU-c <SZhOU-c@users.noreply.github.com>
Co-authored-by: sh1shir <sh1shir@users.noreply.github.com>
Co-authored-by: Rithvik Bhogavilli <rbhogavilli@gmail.com>

* Co-authored-by: sh1shir <sh1shir@users.noreply.github.com>
Co-authored-by: SZhOU-c <SZhOU-c@users.noreply.github.com>

* Co-authored-by: Ishaan Kandamuri <ishaank21@users.noreply.github.com>
Co-authored-by: williamyeh1 <williamyeh1@users.noreply.github.com>
Co-authored-by: sh1shir <sh1shir@users.noreply.github.com>

* Added Thread Sleep

* Implemented Kalman Filter into yessir.cpp

* changed the P_k update equation to match kalmanfilter.net

* reverting the P_k update equation

* addressed Nick's comments

* switched from lowG accel to highG accel

* added the KF reset in `system.cpp`

* redoing the dumb reset i had

* removed `head` function in `Buffer.h`

---------

Co-authored-by: keshavbalaji <keshavbalaji2397@gmail.com>
Co-authored-by: SZhOU-c <SZhOU-c@users.noreply.github.com>
Co-authored-by: sh1shir <sh1shir@users.noreply.github.com>
Co-authored-by: Rithvik Bhogavilli <rbhogavilli@gmail.com>

Author: 4 people

MIDAS/src/Buffer.h

@@ -77,6 +77,27 @@ struct Buffer {
         }
         return arr;
     }
+
+        /**
+     * @brief Reads a range of items into a passed array, which can be larger than the actual count of items.
+     *
+     * @param write_to An array of at least n items to write to
+     * @param start The index to start reading from (inclusive)
+     * @param length the number of items to read
+     * @return How many items were actually read
+     */
+    // TODO make this function return a std::array?
+    size_t readSlice(T write_to[], size_t start, size_t len) {
+        size_t i = 0;
+        size_t idx = oldest_idx() + start;
+        while (i < len) {
+            write_to[i++] = buffer[idx++];
+            if (idx == BUFFER_SIZE) {
+                idx = 0;
+            }
+        }
+        return i;
+    }
 
 
 private:

MIDAS/src/gnc/example_kf.cpp

@@ -2,25 +2,33 @@
 
 ExampleKalmanFilter::ExampleKalmanFilter() : KalmanFilter() {}
 
-void ExampleKalmanFilter::initialize() {}
+void ExampleKalmanFilter::initialize(RocketSystems* args) {}
 
 
 void ExampleKalmanFilter::priori() {
     x_priori = (F_mat * x_k);
     P_priori = (F_mat * P_k * F_mat.transpose()) + Q;
 }
 
-void ExampleKalmanFilter::update() {}
+void ExampleKalmanFilter::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_stat) {}
 
 void ExampleKalmanFilter::setQ(float dt, float sd) {}
 
 void ExampleKalmanFilter::setF(float dt) {}
 
 KalmanData ExampleKalmanFilter::getState() { return KalmanData(); }
 
-void ExampleKalmanFilter::setState(KalmanData state)
+void ExampleKalmanFilter::setState(KalmanState state)
 {
-    this->state = state;
+    this->state.position.px = state.state_est_pos_x;
+    this->state.position.py = state.state_est_pos_y;
+    this->state.position.pz = state.state_est_pos_z;
+    this->state.acceleration.ax = state.state_est_accel_x;
+    this->state.acceleration.ay = state.state_est_accel_y;
+    this->state.acceleration.az = state.state_est_accel_z;
+    this->state.velocity.vx =state.state_est_vel_x;
+    this->state.velocity.vy =state.state_est_vel_y;
+    this->state.velocity.vz =state.state_est_vel_z;
 }
 
 ExampleKalmanFilter example_kf;

MIDAS/src/gnc/example_kf.h

@@ -3,25 +3,27 @@
 #include "kalman_filter.h"
 
 // makes a kalman filter with 9 state variables and 3 sensor inputs
-class ExampleKalmanFilter : public KalmanFilter<9, 3>
+class ExampleKalmanFilter : public KalmanFilter<9, 4>
 {
 public:
     ExampleKalmanFilter();
 
-    void initialize() override;
+    void initialize(RocketSystems* args) override;
+    //virtual void initialize(Orientation &orientation, Barometer &barometer, Acceleration &Acceleration);
     void priori() override;
-    void update() override;
+    void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) override;
+
+    KalmanData getState() override;
+    void setState(KalmanState state) override;
 
     void setQ(float dt, float sd);
     void setF(float dt);
 
-    KalmanData getState() override;
-    void setState(KalmanData state) override;
-
     Eigen::Matrix<float, 3, 1> bodyToGlobal(euler_t angles, Eigen::Matrix<float, 3, 1> x_k);
 
 private:
     KalmanData state;
+    KalmanState kalman_state;
 };
 
-extern ExampleKalmanFilter example_kf;
+extern ExampleKalmanFilter example_kf;

MIDAS/src/gnc/kalman_filter.h

@@ -7,6 +7,20 @@
 
 #include <Eigen/Eigen>
 #include "sensor_data.h"
+#include "systems.h"
+
+
+struct KalmanState {
+    float state_est_pos_x;
+    float state_est_vel_x;
+    float state_est_accel_x;
+    float state_est_pos_y;
+    float state_est_vel_y;
+    float state_est_accel_y;
+    float state_est_pos_z;
+    float state_est_vel_z;
+    float state_est_accel_z;
+};
 
 template <int _NumStates, int _NumInputs>
 class KalmanFilter
@@ -41,11 +55,11 @@ class KalmanFilter
 
         B = Eigen::Matrix<float, _NumStates, _NumInputs>::Zero();
     }
-
-    virtual void initialize() = 0;
+    
+    virtual void initialize(RocketSystems* args) = 0;
     virtual void priori() = 0;
-    virtual void update() = 0;
+    virtual void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState current_state) = 0;
 
     virtual KalmanData getState() = 0;
-    virtual void setState(KalmanData state) = 0;
+    virtual void setState(KalmanState state) = 0;
 };