fixed accelerometer simulation

ctu-mrs · Oct 5, 2023 · 19e8a1a · 19e8a1a
1 parent 015acc0
commit 19e8a1a
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp b/include/mrs_multirotor_simulator/uav_system/multirotor_model.hpp
@@ -276,7 +276,7 @@ void MultirotorModel::step(const double& dt) {
   }
 
   // fabricate what the onboard accelerometer would feel
-  imu_acceleration_ = state_.R.transpose() * (state_.v - state_.v_prev) / dt + Eigen::Vector3d(0, 0, params_.g);
+  imu_acceleration_ = state_.R.transpose() * (((state_.v - state_.v_prev) / dt) + Eigen::Vector3d(0, 0, params_.g));
   state_.v_prev     = state_.v;
 
   // simulate the takeoff patch
@@ -341,6 +341,7 @@ void MultirotorModel::operator()(const MultirotorModel::InternalState& x, Multir
   }
 
   x_dot = cur_state.v;
+
   v_dot = -Eigen::Vector3d(0, 0, params_.g) + thrust * R.col(2) / params_.mass + external_force_ / params_.mass - resistance * vnorm / params_.mass;
 
   R_dot = R * omega_tensor;