diff --git a/Ganesha_II_V2/Core/Inc/bmp581.h b/Ganesha_II_V2/Core/Inc/bmp581.h
index b428472..728ed68 100644
--- a/Ganesha_II_V2/Core/Inc/bmp581.h
+++ b/Ganesha_II_V2/Core/Inc/bmp581.h
@@ -20,7 +20,7 @@
 #define TROPOPAUSE_BASE_ALTITUDE 11000.0f // m
 #define STRATOSPHERE_MIDDLE_BASE_ALTITUDE 20000.0f // m
 
-#define TROPOSPHERE_LAPSE_RATE -0.0065f // K/m
+#define TROPOSPHERE_LAPSE_RATE 0.0065f // K/m
 #define UPPER_STRATOSPHERE_LAPSE_RATE 0.001f // K/m
 
 struct BMP581 {
@@ -28,6 +28,7 @@ struct BMP581 {
 	struct bmp5_osr_odr_press_config odr_config;
 	struct bmp5_int_source_select int_config;
 	I2C_HandleTypeDef *hi2c;
+	float starting_altitude;
 };
 
 BMP5_INTF_RET_TYPE read_i2c(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr);
@@ -35,6 +36,7 @@ BMP5_INTF_RET_TYPE write_i2c(uint8_t reg_addr, const uint8_t *reg_data, uint32_t
 int8_t bmp581_init(struct BMP581 *bmp581, I2C_HandleTypeDef *handle);
 int8_t bmp581_update_data(struct BMP581 *bmp581, struct bmp5_sensor_data *data);
 int8_t bmp581_get_power_mode(struct BMP581 *bmp581, enum bmp5_powermode *powermode);
-float bmp581_estimate_altitude_msl(struct BMP581 *bmp581, struct bmp5_sensor_data *data);
+void bmp581_init_altitude_estimator(struct BMP581 *bmp581, struct bmp5_sensor_data *data);
+float bmp581_estimate_altitude_relative(struct BMP581 *bmp581, struct bmp5_sensor_data *data);
 
 #endif
diff --git a/Ganesha_II_V2/Core/Src/bmp581.c b/Ganesha_II_V2/Core/Src/bmp581.c
index 3e7afc6..f9c1417 100644
--- a/Ganesha_II_V2/Core/Src/bmp581.c
+++ b/Ganesha_II_V2/Core/Src/bmp581.c
@@ -80,39 +80,40 @@ int8_t bmp581_update_data(struct BMP581 *bmp581, struct bmp5_sensor_data *data)
 	return bmp5_get_sensor_data(data, &(bmp581->odr_config), &(bmp581->device));
 }
 
-static inline float calc_altitude_troposphere_msl(float pressure) {
-    float exponent = (-GAS_CONSTANT * TROPOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS);
-    float pressure_ratio = pressure / STANDARD_SEA_LEVEL_PRESSURE;
-    float power_term = powf(pressure_ratio, exponent) - 1;
-
-    return (STANDARD_SEA_LEVEL_TEMP / TROPOSPHERE_LAPSE_RATE) * power_term;
+static inline float troposphere_calc(float pressure) {
+    const float exponent = (-GAS_CONSTANT * TROPOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS);
+    return (STANDARD_SEA_LEVEL_TEMP / TROPOSPHERE_LAPSE_RATE) * (powf(pressure / STANDARD_SEA_LEVEL_PRESSURE, exponent) - 1.0f);
 }
 
-static inline float calc_altitude_lower_stratosphere_msl(float pressure) {
-    float log_ratio = logf(pressure / TROPOPAUSE_PRESSURE);
-    float scale_factor = (GAS_CONSTANT * STRATOSPHERE_BASE_TEMP) / (GRAVITY_ACCEL * AIR_MOLAR_MASS);
-
-    return TROPOPAUSE_BASE_ALTITUDE - (scale_factor * log_ratio);
+static inline float lower_stratosphere_calc(float pressure) {
+    const float scale_height = (GAS_CONSTANT * STRATOSPHERE_BASE_TEMP) / (GRAVITY_ACCEL * AIR_MOLAR_MASS);
+    return TROPOPAUSE_BASE_ALTITUDE + scale_height * logf(TROPOPAUSE_PRESSURE / pressure);
 }
 
-static inline float calc_altitude_upper_stratosphere_msl(float pressure) {
-    float exponent = (-GAS_CONSTANT * UPPER_STRATOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS);
-    float pressure_ratio = pressure / STRATOSPHERE_MIDDLE_PRESSURE;
-    float power_term = powf(pressure_ratio, exponent) - 1;
-
-    return STRATOSPHERE_MIDDLE_BASE_ALTITUDE + (STRATOSPHERE_BASE_TEMP / UPPER_STRATOSPHERE_LAPSE_RATE) * power_term;
+static inline float upper_stratosphere_calc(float pressure) {
+    const float exponent = (-GAS_CONSTANT * UPPER_STRATOSPHERE_LAPSE_RATE) / (GRAVITY_ACCEL * AIR_MOLAR_MASS);
+    const float term = powf(pressure / STRATOSPHERE_MIDDLE_PRESSURE, exponent) - 1.0f;
+    return STRATOSPHERE_MIDDLE_BASE_ALTITUDE + (STRATOSPHERE_BASE_TEMP / UPPER_STRATOSPHERE_LAPSE_RATE) * term;
 }
 
-float bmp581_estimate_altitude_msl(struct BMP581 *bmp581, struct bmp5_sensor_data *data) {
-    if (data->pressure > TROPOPAUSE_PRESSURE) {
-        return calc_altitude_troposphere_msl(data->pressure);
-    } else if (data->pressure > STRATOSPHERE_MIDDLE_PRESSURE) {
-        return calc_altitude_lower_stratosphere_msl(data->pressure);
+static inline float pressure_to_isa_altitude(float pressure) {
+    if (pressure > TROPOPAUSE_PRESSURE) {
+        return troposphere_calc(pressure);
+    } else if (pressure > STRATOSPHERE_MIDDLE_PRESSURE) {
+        return lower_stratosphere_calc(pressure);
     } else {
-        return calc_altitude_upper_stratosphere_msl(data->pressure);
+        return upper_stratosphere_calc(pressure);
     }
 }
 
+void bmp581_init_altitude_estimator(struct BMP581 *bmp581, struct bmp5_sensor_data *data) {
+	bmp581->starting_altitude = pressure_to_isa_altitude(data->pressure);
+}
+
+float bmp581_estimate_altitude_relative(struct BMP581 *bmp581, struct bmp5_sensor_data *data) {
+	return pressure_to_isa_altitude(data->pressure) - bmp581->starting_altitude;
+}
+
 int8_t bmp581_get_power_mode(struct BMP581 *bmp581, enum bmp5_powermode *powermode) {
 	return bmp5_get_power_mode(powermode, &(bmp581->device));
 }
diff --git a/Ganesha_II_V2/Core/Src/main.c b/Ganesha_II_V2/Core/Src/main.c
index 7e59e2c..3c5e225 100644
--- a/Ganesha_II_V2/Core/Src/main.c
+++ b/Ganesha_II_V2/Core/Src/main.c
@@ -175,8 +175,9 @@ void HAL_GPIO_EXTI_Callback(uint16_t pin) {
 }
 
 struct Orientation_Estimator estimator;
-uint8_t estimator_init = 0;
+uint8_t orientation_estimator_initialized = 0;
 
+uint8_t altitude_estimator_initialized = 0;
 /* USER CODE END 0 */
 
 /**
@@ -318,9 +319,9 @@ int main(void)
 		  packet.angular_velocity_y_rads = bmi088_convert_gyro_axis_data(&bmi088, bmi088_gyro_data.y);
 		  packet.angular_velocity_z_rads = bmi088_convert_gyro_axis_data(&bmi088, bmi088_gyro_data.z);
 
-		  if (!estimator_init) {
+		  if (!orientation_estimator_initialized) {
 			  orientation_estimator_reset_from_accel(&estimator, packet.acceleration_x_mss, packet.acceleration_y_mss, packet.acceleration_z_mss);
-			  estimator_init = 1;
+			  orientation_estimator_initialized = 1;
 		  }
 
 		  orientation_estimator_add_gyro_reading(
@@ -347,7 +348,12 @@ int main(void)
 		  bmp581_update_data(&bmp581, &bmp_data);
 	  }
 
-	  packet.barometer_hMSL_m = bmp581_estimate_altitude_msl(&bmp581, &bmp_data);
+	  if (!altitude_estimator_initialized && (bmp_data.pressure && bmp_data.temperature)) {
+		  bmp581_init_altitude_estimator(&bmp581, &bmp_data);
+		  altitude_estimator_initialized = 1;
+	  }
+
+	  packet.barometer_hMSL_m = bmp581_estimate_altitude_relative(&bmp581, &bmp_data);
 	  packet.temperature_c = bmp_data.temperature;
 
 	  if (dumb_timer_done(&camera_timer)) {