fix acceleration from read_imu for euler method

src/lib.rs

@@ -61,6 +61,8 @@ pub struct Quadrotor {
     pub position: Vector3<f32>,
     /// Current velocity of the quadrotor
     pub velocity: Vector3<f32>,
+    /// Current acceleration of the quadrotor
+    pub acceleration: Vector3<f32>,
     /// Current orientation of the quadrotor
     pub orientation: UnitQuaternion<f32>,
     /// Current angular velocity of the quadrotor
@@ -117,6 +119,7 @@ impl Quadrotor {
         Ok(Self {
             position: Vector3::zeros(),
             velocity: Vector3::zeros(),
+            acceleration: Vector3::zeros(),
             orientation: UnitQuaternion::identity(),
             angular_velocity: Vector3::zeros(),
             mass,
@@ -151,8 +154,8 @@ impl Quadrotor {
         let gravity_force = Vector3::new(0.0, 0.0, -self.mass * self.gravity);
         let drag_force = -self.drag_coefficient * self.velocity.norm() * self.velocity;
         let thrust_world = self.orientation * Vector3::new(0.0, 0.0, control_thrust);
-        let acceleration = (thrust_world + gravity_force + drag_force) / self.mass;
-        self.velocity += acceleration * self.time_step;
+        self.acceleration = (thrust_world + gravity_force + drag_force) / self.mass;
+        self.velocity += self.acceleration * self.time_step;
         self.position += self.velocity * self.time_step;
         let inertia_angular_velocity = self.inertia_matrix * self.angular_velocity;
         let gyroscopic_torque = self.angular_velocity.cross(&inertia_angular_velocity);
@@ -327,10 +330,7 @@ impl Quadrotor {
     /// let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap();
     /// ```
     pub fn read_imu(&self) -> Result<(Vector3<f32>, Vector3<f32>), SimulationError> {
-        let gravity_world = Vector3::new(0.0, 0.0, self.gravity);
-        let true_acceleration =
-            self.orientation.inverse() * (self.velocity / self.time_step - gravity_world);
-        Ok((true_acceleration, self.angular_velocity))
+        Ok((self.acceleration, self.angular_velocity))
     }
 }
 /// Represents an Inertial Measurement Unit (IMU) with bias and noise characteristics