diff --git a/libraries/SITL/SIM_BalanceBot.cpp b/libraries/SITL/SIM_BalanceBot.cpp
index f3b11d0c90..56b02f6205 100644
--- a/libraries/SITL/SIM_BalanceBot.cpp
+++ b/libraries/SITL/SIM_BalanceBot.cpp
@@ -95,7 +95,13 @@ void BalanceBot::update(const struct sitl_input &input)
     float theta = p; //radians
 
     float ang_vel = gyro.y; //radians/s
-    
+    if (!hal.util->get_soft_armed()) {
+        // simulated fingers uprighting the vehicle
+        const float p_gain = 200;
+        const float pitch_response = -sin(p) * p_gain * delta_time;
+        ang_vel += pitch_response;
+    }
+
     // t1,t2,t3 are terms in the equation to find vehicle frame x acceleration
     const float t1 = ((2.0f*gear_ratio*k_t*v/(R*r_w)) - (2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w*r_w)) - (m_p*l*ang_vel*ang_vel*sin(theta))) * (i_p + m_p*l*l);
     const float t2 = -m_p*l*cos(theta)*((2.0f*gear_ratio*k_t*k_e*velocity_vf_x/(R*r_w)) - (2.0f*gear_ratio*k_t*v/(R)) + (m_p*GRAVITY_MSS*l*sin(theta)));
@@ -134,7 +140,8 @@ void BalanceBot::update(const struct sitl_input &input)
     // we are on the ground, so our vertical accel is zero
     accel_earth.z = 0;
 
-    if (!hal.util->get_soft_armed()) {
+    if (!hal.util->get_soft_armed() &&
+        p < radians(2)) {
         // reset to vertical when not armed for faster testing
         accel_earth.zero();
         velocity_ef.zero();