diff --git a/applications/app_balance.c b/applications/app_balance.c
index 2527c87b..81ba1930 100644
--- a/applications/app_balance.c
+++ b/applications/app_balance.c
@@ -91,8 +91,7 @@ static float proportional, integral, derivative;
 static float last_proportional;
 static float pid_value;
 static float setpoint, setpoint_target, setpoint_target_interpolated;
-static uint16_t wheel_diameter;
-static float turn_radius, turn_circumference, turn_angle, abs_roll_angle, roll_compensation, roll_step_size;
+static float abs_roll_angle, roll_angle_sin, roll_compensation, roll_step_size;
 static SetpointAdjustmentType setpointAdjustmentType;
 static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
 static systime_t current_time, last_time, diff_time;
@@ -111,7 +110,6 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
 	}
 	startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
 	tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
-    wheel_diameter = mc_interface_get_configuration()->si_wheel_diameter;
 }
 
 void app_balance_start(void) {
@@ -134,11 +132,9 @@ void reset_vars(void){
 	setpoint_target = 0;
 	setpointAdjustmentType = CENTERING;
 	yaw_setpoint = 0;
+	roll_angle_sin = 0;
     roll_step_size = 0;
     roll_compensation = 0;
-    turn_radius = 0;
-    turn_circumference = 0;
-    turn_angle = 0;
 	state = RUNNING;
 	current_time = 0;
 	last_time = 0;
@@ -287,15 +283,14 @@ void calculate_setpoint_target(void){
 }
 
 void calculate_roll_compensation(void){
-    if(setpointAdjustmentType != CENTERING && (state == RUNNING || state == RUNNING_TILTBACK_CONSTANT) && abs_roll_angle > balance_conf.yaw_ki && abs_erpm > balance_conf.roll_steer_erpm_kp){
-        turn_radius = cosf(abs_roll_angle * M_PI / 180.0f) * wheel_diameter;
-        turn_circumference = (2 * M_PI) * turn_radius;
-        turn_angle = ((((mc_interface_get_speed() * 1000) / balance_conf.hertz) / turn_circumference) * 360.0f) / 2.0f;
+    if(setpointAdjustmentType != CENTERING && (state == RUNNING || state == RUNNING_TILTBACK_CONSTANT) && abs_roll_angle > balance_conf.yaw_ki && abs_erpm > balance_conf.yaw_kd){
+        roll_angle_sin = sinf(abs_roll_angle * M_PI / 180.0f);
+        roll_compensation = (roll_angle_sin * balance_conf.yaw_current_clamp) + ((abs_erpm * balance_conf.roll_steer_erpm_kp) / 60);
 
         if(erpm > 0){
-            roll_compensation = (balance_conf.roll_steer_kp * turn_angle);
+            roll_compensation = balance_conf.roll_steer_kp * roll_compensation;
         } else{
-            roll_compensation = (-balance_conf.roll_steer_kp * turn_angle);
+            roll_compensation = -balance_conf.roll_steer_kp * roll_compensation;
         }
 
         if(roll_compensation > balance_conf.yaw_current_clamp){