diff --git a/MIDAS/src/Buffer.h b/MIDAS/src/Buffer.h
index 83d4907d..bd1cc2d1 100644
--- a/MIDAS/src/Buffer.h
+++ b/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:
diff --git a/MIDAS/src/gnc/example_kf.cpp b/MIDAS/src/gnc/example_kf.cpp
index 04b36c49..ccfa6775 100644
--- a/MIDAS/src/gnc/example_kf.cpp
+++ b/MIDAS/src/gnc/example_kf.cpp
@@ -2,7 +2,7 @@
 
 ExampleKalmanFilter::ExampleKalmanFilter() : KalmanFilter() {}
 
-void ExampleKalmanFilter::initialize() {}
+void ExampleKalmanFilter::initialize(RocketSystems* args) {}
 
 
 void ExampleKalmanFilter::priori() {
@@ -10,7 +10,7 @@ void ExampleKalmanFilter::priori() {
     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) {}
 
@@ -18,9 +18,17 @@ 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;
diff --git a/MIDAS/src/gnc/example_kf.h b/MIDAS/src/gnc/example_kf.h
index 86f27ce7..ba28f69d 100644
--- a/MIDAS/src/gnc/example_kf.h
+++ b/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;
\ No newline at end of file
+extern ExampleKalmanFilter example_kf;
diff --git a/MIDAS/src/gnc/kalman_filter.h b/MIDAS/src/gnc/kalman_filter.h
index 1041c042..cef1379e 100644
--- a/MIDAS/src/gnc/kalman_filter.h
+++ b/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;
 };
diff --git a/MIDAS/src/gnc/yessir.cpp b/MIDAS/src/gnc/yessir.cpp
index 760dfff6..402b9932 100644
--- a/MIDAS/src/gnc/yessir.cpp
+++ b/MIDAS/src/gnc/yessir.cpp
@@ -1,47 +1,325 @@
 #include "yessir.h"
+#include "finite-state-machines/fsm_states.h"
 
 Yessir::Yessir() : KalmanFilter() {
     state = KalmanData();
 }
 
-void Yessir::initialize() {}
+/**
+ * @brief Sets altitude by averaging 30 barometer measurements taken 100 ms
+ * apart
+ *
+ * The following for loop takes a series of barometer measurements on start
+ * up and takes the average of them in order to initialize the kalman filter
+ * to the correct initial barometric altitude. This is done so that the
+ * kalman filter takes minimal time to converge to an accurate state
+ * estimate. This process is significantly faster than allowing the state as
+ * letting the filter to converge to the correct state can take up to 3 min.
+ * This specific process was used because the barometric altitude will
+ * change depending on the weather and thus, the initial state estimate
+ * cannot be hard coded. A GPS altitude may be used instead but due to GPS
+ * losses during high speed/high altitude flight, it is inadvisable with the
+ * current hardware to use this as a solution. Reference frames should also
+ * be kept consistent (do not mix GPS altitude and barometric).
+ *
+ */
 
-void Yessir::priori() {}
+void Yessir::initialize(RocketSystems* args) {
+    Orientation orientation = args->rocket_data.orientation.getRecentUnsync();
+    
+    float sum = 0;
+    
+    for (int i = 0; i < 30; i++) {
+        Barometer barometer = args->rocket_data.barometer.getRecent();
+        LowGData initial_accelerometer = args->rocket_data.low_g.getRecent();
+        Acceleration accelerations = {
+            .ax = initial_accelerometer.ax,
+            .ay = initial_accelerometer.ay,
+            .az = initial_accelerometer.az
+        };
+        sum += barometer.altitude;
+
+        init_accel(0, 0) += accelerations.az;
+        init_accel(1, 0) += accelerations.ay;
+        init_accel(2, 0) += -accelerations.ax;
+        THREAD_SLEEP(100);
+    }
+
+    init_accel(0, 0) /= 30;
+    init_accel(1, 0) /= 30;
+    init_accel(2, 0) /= 30;
+
+    euler_t euler = orientation.getEuler();
+    euler.yaw = -euler.yaw;
+    world_accel = BodyToGlobal(euler, init_accel);
+
+    // set x_k
+    x_k(0, 0) = sum / 30;
+    x_k(3, 0) = 0;
+    x_k(6, 0) = 0;
+
+    // set F
+    for (int i = 0; i < 3; i++) {
+        F_mat(3 * i, 3 * i + 1) = s_dt;
+        F_mat(3 * i, 3 * i + 2) = (s_dt * s_dt) / 2;
+        F_mat(3 * i + 1, 3 * i + 2) = s_dt;
+
+        F_mat(3 * i, 3 * i) = 1;
+        F_mat(3 * i + 1, 3 * i + 1) = 1;
+        F_mat(3 * i + 2, 3 * i + 2) = 1;
+    }
+
+    Q(0, 0) = pow(s_dt, 5) / 20;
+    Q(0, 1) = pow(s_dt, 4) / 8;
+    Q(0, 2) = pow(s_dt, 3) / 6;
+    Q(1, 1) = pow(s_dt, 3) / 8;
+    Q(1, 2) = pow(s_dt, 2) / 2;
+    Q(2, 2) = s_dt;
+    Q(1, 0) = Q(0, 1);
+    Q(2, 0) = Q(0, 2);
+    Q(2, 1) = Q(1, 2);
 
-void Yessir::update() {}
+    Q(3, 3) = pow(s_dt, 5) / 20;
+    Q(3, 4) = pow(s_dt, 4) / 8;
+    Q(3, 5) = pow(s_dt, 3) / 6;
+    Q(4, 4) = pow(s_dt, 3) / 8;
+    Q(4, 5) = pow(s_dt, 2) / 2;
+    Q(5, 5) = s_dt;
+    Q(4, 3) = Q(3, 4);
+    Q(5, 3) = Q(3, 5);
+    Q(5, 4) = Q(4, 5);
 
-void Yessir::tick(float dt, float sd, Barometer &barometerData, Acceleration &imuData) {
-    // double integrate accelerometer data and store in state struct
-    state.acceleration = {
-        .ax = imuData.ax * 9.8f - 9.8f,
-        .ay = imuData.ay * 9.8f,
-        .az = imuData.az * 9.8f
-    };
+    Q(6, 6) = pow(s_dt, 5) / 20;
+    Q(6, 7) = pow(s_dt, 4) / 8;
+    Q(6, 8) = pow(s_dt, 3) / 6;
+    Q(7, 7) = pow(s_dt, 3) / 8;
+    Q(7, 8) = pow(s_dt, 2) / 2;
+    Q(8, 8) = s_dt;
+    Q(7, 6) = Q(6, 7);
+    Q(8, 6) = Q(6, 8);
+    Q(8, 7) = Q(7, 8);
 
-    state.velocity = {
-        .vx = state.velocity.vx + state.acceleration.ax * dt,
-        .vy = state.velocity.vy + state.acceleration.ay * dt,
-        .vz = state.velocity.vz + state.acceleration.az * dt
-    };
+    // set H
+    H(0, 0) = 1;
+    H(1, 2) = 1;
+    H(2, 5) = 1;
+    H(3, 8) = 1;
 
-    state.position = {
-        .px = state.position.px + state.velocity.vx * dt + 0.5f * state.acceleration.ax * dt * dt,
-        .py = state.position.py + state.velocity.vy * dt + 0.5f * state.acceleration.ay * dt * dt,
-        .pz = state.position.pz + state.velocity.vz * dt + 0.5f * state.acceleration.az * dt * dt
-    };
+    Q = Q * spectral_density_;
+
+    // set R
+    R(0, 0) = 2.0;
+    R(1, 1) = 1.9;
+    R(2, 2) = 10;
+    R(3, 3) = 10;
+
+    // set B (don't care about what's in B since we have no control input)
+    B(2, 0) = -1;
+}
+
+/**
+ * @brief Estimates current state of the rocket without current sensor data
+ *
+ * The priori step of the Kalman filter is used to estimate the current state
+ * of the rocket without knowledge of the current sensor data. In other words,
+ * it extrapolates the state at time n+1 based on the state at time n.
+ */
+void Yessir::priori() {
+     // x_priori = (F @ x_k) + ((B @ u).T) #* For some reason doesnt work when B
+    // or u is = 0
+    x_priori = (F_mat * x_k);
+    P_priori = (F_mat * P_k * F_mat.transpose()) + Q;
+}
+
+/**
+ * @brief Update Kalman Gain and state estimate with current sensor data
+ *
+ * After receiving new sensor data, the Kalman filter updates the state estimate
+ * and Kalman gain. The Kalman gain can be considered as a measure of how uncertain
+ * the new sensor data is. After updating the gain, the state estimate is updated.
+ *
+ */
+void Yessir::update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState FSM_state) {
+    if (FSM_state == FSMState::STATE_FIRST_BOOST || FSM_state == FSMState::STATE_SECOND_BOOST) { 
+        float sum = 0;
+        float data[10];
+        alt_buffer.readSlice(data, 0, 10);
+        for (float i : data) {
+            sum += i;
+        }
+        KalmanState kalman_state = (KalmanState){sum / 10.0f, 0, 0, 0, 0, 0, 0, 0, 0};
+        setState(kalman_state);
+    } else if (FSM_state >= FSMState::STATE_APOGEE) {
+        H(1, 2) = 0;
+    }
+
+    Eigen::Matrix<float, 4, 4> S_k = Eigen::Matrix<float, 4, 4>::Zero();
+    S_k = (((H * P_priori * H.transpose()) + R)).inverse();
+    Eigen::Matrix<float, 9, 9> identity = Eigen::Matrix<float, 9, 9>::Identity();
+    K = (P_priori * H.transpose()) * S_k;
+
+    // Sensor Measurements
+    Eigen::Matrix<float, 3, 1> accel = Eigen::Matrix<float, 3, 1>::Zero();
     
+    accel(0, 0) = acceleration.az - 0.045;
+    accel(1, 0) = acceleration.ay - 0.065;
+    accel(2, 0) = -acceleration.ax - 0.06;
+
+    euler_t angles = orientation.getEuler();
+    angles.yaw = -angles.yaw;
+
+    Eigen::Matrix<float, 3, 1> acc = BodyToGlobal(angles, accel);
+
+    y_k(1, 0) = (acc(0)) * 9.81 - 9.81;
+    y_k(2, 0) = (acc(1)) * 9.81;
+    y_k(3, 0) = (acc(2)) * 9.81;
+
+    y_k(0, 0) = barometer.altitude;
+    alt_buffer.push(barometer.altitude);
+
+
+    // # Posteriori Update
+    x_k = x_priori + K * (y_k - (H * x_priori));
+    P_k = (identity - K * H) * P_priori;
+    // Joseph (Expanded) Form
+    // P_k = (identity - K * H) * P_priori * (identity - K * H).transpose() + K * R * K.transpose(); 
+    
+    kalman_state.state_est_pos_x = x_k(0, 0);
+    kalman_state.state_est_vel_x = x_k(1, 0);
+    kalman_state.state_est_accel_x = x_k(2, 0);
+    kalman_state.state_est_pos_y = x_k(3, 0);
+    kalman_state.state_est_vel_y = x_k(4, 0);
+    kalman_state.state_est_accel_y = x_k(5, 0);
+    kalman_state.state_est_pos_z = x_k(6, 0);
+    kalman_state.state_est_vel_z = x_k(7, 0);
+    kalman_state.state_est_accel_z = x_k(8, 0);
+
+    state.position = (Position){kalman_state.state_est_pos_x,kalman_state.state_est_pos_y,kalman_state.state_est_pos_z};
+    state.velocity = (Velocity){kalman_state.state_est_vel_x,kalman_state.state_est_vel_y,kalman_state.state_est_vel_z};
+    state.acceleration = (Acceleration){kalman_state.state_est_accel_x,kalman_state.state_est_accel_y,kalman_state.state_est_accel_z};
+}
+
+/**
+ * @brief Run Kalman filter calculations as long as FSM has passed IDLE
+ *
+ * @param dt Time step calculated by the Kalman Filter Thread
+ * @param sd Spectral density of the noise
+ * @param &barometer Data of the current barometer
+ * @param acceleration Current acceleration
+ * @param &orientation Current orientation
+ * @param current_state Current FSM_state
+ */
+void Yessir::tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState FSM_state) {
+    if (FSM_state >= FSMState::STATE_IDLE) {
+        setF(dt / 1000);
+        setQ(dt / 1000, sd);
+        priori();
+        update(barometer, acceleration, orientation, FSM_state);
+    }
 }
 
+/**
+ * @brief Converts a vector in the body frame to the global frame
+ *
+ * @param angles Roll, pitch, yaw angles
+ * @param body_vect Vector for rotation in the body frame
+ * @return Eigen::Matrix<float, 3, 1> Rotated vector in the global frame
+ */
+Eigen::Matrix<float, 3, 1> Yessir::BodyToGlobal(euler_t angles, Eigen::Matrix<float, 3, 1> body_vect) {
+    Eigen::Matrix3f roll, pitch, yaw;
+    roll << 1., 0., 0., 0., cos(angles.roll), -sin(angles.roll), 0., sin(angles.roll), cos(angles.roll);
+    pitch << cos(angles.pitch), 0., sin(angles.pitch), 0., 1., 0., -sin(angles.pitch), 0., cos(angles.pitch);
+    yaw << cos(angles.yaw), -sin(angles.yaw), 0., sin(angles.yaw), cos(angles.yaw), 0., 0., 0., 1.;
+    return yaw * pitch * body_vect;
+}
+
+
+/**
+ * @brief Getter for state X
+ *
+ * @return the current state, see sensor_data.h for KalmanData
+ */
 KalmanData Yessir::getState() {
     return state;
 }
 
-void Yessir::setState(KalmanData state) {
-    this->state = state;
+/**
+ * @brief Sets state vector x
+ *
+ * @param state Wanted state vector
+ */
+void Yessir::setState(KalmanState 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;
+}
+
+/**
+ * @brief Sets the Q matrix given time step and spectral density.
+ *
+ * @param dt Time step calculated by the Kalman Filter Thread
+ * @param sd Spectral density of the noise
+ *
+ * The Q matrix is the covariance matrix for the process noise and is
+ * updated based on the time taken per cycle of the Kalman Filter Thread.
+ */
+void Yessir::setQ(float dt, float sd) {
+    Q(0, 0) = pow(dt, 5) / 20;
+    Q(0, 1) = pow(dt, 4) / 8;
+    Q(0, 2) = pow(dt, 3) / 6;
+    Q(1, 1) = pow(dt, 3) / 8;
+    Q(1, 2) = pow(dt, 2) / 2;
+    Q(2, 2) = dt;
+    Q(1, 0) = Q(0, 1);
+    Q(2, 0) = Q(0, 2);
+    Q(2, 1) = Q(1, 2);
+    Q(3, 3) = pow(dt, 5) / 20;
+    Q(3, 4) = pow(dt, 4) / 8;
+    Q(3, 5) = pow(dt, 3) / 6;
+    Q(4, 4) = pow(dt, 3) / 8;
+    Q(4, 5) = pow(dt, 2) / 2;
+    Q(5, 5) = dt;
+    Q(4, 3) = Q(3, 4);
+    Q(5, 3) = Q(3, 5);
+    Q(5, 4) = Q(4, 5);
+
+    Q(6, 6) = pow(dt, 5) / 20;
+    Q(6, 7) = pow(dt, 4) / 8;
+    Q(6, 8) = pow(dt, 3) / 6;
+    Q(7, 7) = pow(dt, 3) / 8;
+    Q(7, 8) = pow(dt, 2) / 2;
+    Q(8, 8) = dt;
+    Q(7, 6) = Q(6, 7);
+    Q(8, 6) = Q(6, 8);
+    Q(8, 7) = Q(7, 8);
+
+    Q *= sd;
 }
 
-void Yessir::setQ(float dt, float sd) {}
+/**
+ * @brief Sets the F matrix given time step.
+ *
+ * @param dt Time step calculated by the Kalman Filter Thread
+ *
+ * The F matrix is the state transition matrix and is defined
+ * by how the states change over time.
+ */
+void Yessir::setF(float dt) {
+    for (int i = 0; i < 3; i++) {
+        F_mat(3 * i, 3 * i + 1) = s_dt;
+        F_mat(3 * i, 3 * i + 2) = (dt * s_dt) / 2;
+        F_mat(3 * i + 1, 3 * i + 2) = s_dt;
 
-void Yessir::setF(float dt) {}
+        F_mat(3 * i, 3 * i) = 1;
+        F_mat(3 * i + 1, 3 * i + 1) = 1;
+        F_mat(3 * i + 2, 3 * i + 2) = 1;
+    }
+}
 
 Yessir yessir;
diff --git a/MIDAS/src/gnc/yessir.h b/MIDAS/src/gnc/yessir.h
index 25779878..15e74a81 100644
--- a/MIDAS/src/gnc/yessir.h
+++ b/MIDAS/src/gnc/yessir.h
@@ -2,28 +2,39 @@
 
 #include "kalman_filter.h"
 #include "sensor_data.h"
+#include "Buffer.h"
 
-class Yessir : public KalmanFilter<3, 3>
+#define NUM_STATES 9
+#define NUM_SENSOR_INPUTS 4
+#define ALTITUDE_BUFFER_SIZE 10
+
+class Yessir : public KalmanFilter<NUM_STATES, NUM_SENSOR_INPUTS>
 {
 public:
     Yessir();
-
-    void initialize() override;
-    void priori() override;
-    void update() override;
+    void initialize(RocketSystems* args) override; 
+    void priori() override; 
+    void update(Barometer barometer, Acceleration acceleration, Orientation orientation, FSMState state) override;
 
     void setQ(float dt, float sd);
-    void setF(float dt);
+    void setF(float dt); 
+    Eigen::Matrix<float, 3, 1> BodyToGlobal(euler_t angles, Eigen::Matrix<float, 3, 1> x_k);
 
     KalmanData getState() override;
-    void setState(KalmanData state) override;
+    void setState(KalmanState state) override;
 
-    void tick(float dt, float sd, Barometer &barometerData, Acceleration &imuData);
+    void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state);
+   
 
 private:
-    float s_dt_ = 0.05f;
+    float s_dt = 0.05f;
     float spectral_density_ = 13.0f;
+    float kalman_apo = 0;
+    KalmanState kalman_state;
 
+    Eigen::Matrix<float, 3, 1> init_accel = Eigen::Matrix<float, 3, 1>::Zero();
+    Eigen::Matrix<float, 3, 1> world_accel;
+    Buffer<float, ALTITUDE_BUFFER_SIZE> alt_buffer;
     KalmanData state;
 };
 
diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h
index d6b8b765..fe13f610 100644
--- a/MIDAS/src/sensor_data.h
+++ b/MIDAS/src/sensor_data.h
@@ -181,6 +181,15 @@ struct Orientation {
     float pitch = 0;
     float roll = 0;
 
+    //For yessir.cpp
+    euler_t getEuler() const {
+        euler_t euler;
+        euler.yaw = this->yaw;
+        euler.pitch = this->pitch;
+        euler.roll = this->roll;
+        return euler;
+    }
+
     Velocity orientation_velocity;
     Acceleration orientation_acceleration;
 
diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp
index 4dd3b864..5a144803 100644
--- a/MIDAS/src/systems.cpp
+++ b/MIDAS/src/systems.cpp
@@ -125,13 +125,19 @@ DECLARE_THREAD(buzzer, RocketSystems* arg) {
 }
 
 DECLARE_THREAD(kalman, RocketSystems* arg) {
-    yessir.initialize();
+    // Orientation initial_orientation = arg->rocket_data.orientation.getRecent();
+    // Barometer initial_barom_buf = arg->rocket_data.barometer.getRecent();
+    // LowGData initial_accelerometer = arg->rocket_data.low_g.getRecent();
+    //yessir.initialize(initial_orientation, initial_barom_buf, initial_accelerations);
+    yessir.initialize(arg);
     TickType_t last = xTaskGetTickCount();
-
+    
     while (true) {
         // add the tick update function
-        Barometer current_barom_buf = arg->rocket_data.barometer.getRecentUnsync();
-        LowGData current_accelerometer = arg->rocket_data.low_g.getRecentUnsync();
+        Barometer current_barom_buf = arg->rocket_data.barometer.getRecent();
+        Orientation current_orientation = arg->rocket_data.orientation.getRecent();
+        HighGData current_accelerometer = arg->rocket_data.high_g.getRecent();
+        FSMState FSM_state = arg->rocket_data.fsm_state.getRecent();
         Acceleration current_accelerations = {
             .ax = current_accelerometer.ax,
             .ay = current_accelerometer.ay,
@@ -139,7 +145,7 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
         };
         float dt = pdTICKS_TO_MS(xTaskGetTickCount() - last) / 1000.0f;
 
-        yessir.tick(dt, 13.0, current_barom_buf, current_accelerations);
+        yessir.tick(dt, 13.0, current_barom_buf, current_accelerations, current_orientation, FSM_state);
         KalmanData current_state = yessir.getState();
 
         arg->rocket_data.kalman.update(current_state);