Compute angular accelerations using numerical differentiation

This PR implements the possibility to compute the angular accelerations from nummerically differentiating the angular velocities using second order accurate central differences. The computation of the angular acceleration is applied before the resampling and filtering on the raw data.

This is needed for all flight logs since the angular acceleration topic is only present in logs from the sim.

Author: manumerous

Tools/parametric_model/configs/quadplane_model.yaml

@@ -104,6 +104,7 @@ dynamics_model_config:
   estimate_forces: True
   estimate_moments: True
   resample_freq: 100.0
+  estimate_angular_acceleration: False
   data:
     required_ulog_topics:
       actuator_outputs:

Tools/parametric_model/configs/quadrotor_model.yaml

@@ -89,8 +89,21 @@ dynamics_model_config:
   estimate_forces: True
   estimate_moments: True
   resample_freq: 100.0
+  estimate_angular_acceleration: True
   data:
     required_ulog_topics:
+      # Use with simulator logs only, disable estimate_angular_acceleration when using
+      # vehicle_angular_acceleration:
+      #   ulog_name:
+      #     - "timestamp"
+      #     - "xyz[0]"
+      #     - "xyz[1]"
+      #     - "xyz[2]"
+      #   dataframe_name:
+      #     - "timestamp"
+      #     - "ang_acc_b_x"
+      #     - "ang_acc_b_y"
+      #     - "ang_acc_b_z"
       actuator_outputs:
         ulog_name:
           - "timestamp"
@@ -151,14 +164,4 @@ dynamics_model_config:
           - "acc_b_x"
           - "acc_b_y"
           - "acc_b_z"
-      vehicle_angular_acceleration:
-        ulog_name:
-          - "timestamp"
-          - "xyz[0]"
-          - "xyz[1]"
-          - "xyz[2]"
-        dataframe_name:
-          - "timestamp"
-          - "ang_acc_b_x"
-          - "ang_acc_b_y"
-          - "ang_acc_b_z"
+      

Tools/parametric_model/configs/standardplane_model.yaml

@@ -107,6 +107,7 @@ dynamics_model_config:
   estimate_forces: True
   estimate_moments: True
   resample_freq: 50.0
+  estimate_angular_acceleration: False
   data:
     required_ulog_topics:
       actuator_outputs:

Tools/parametric_model/src/tools/data_handler.py

@@ -71,6 +71,7 @@ def __init__(self, config_file):
         assert bool(config_dict), 'req_topics_dict can not be empty'
         self.config_dict = config_dict
         self.resample_freq = config_dict["resample_freq"]
+        self.estimate_angular_acceleration = config_dict["estimate_angular_acceleration"]
         print("Resample frequency: ", self.resample_freq, "Hz")
         self.req_topics_dict = config_dict["data"]["required_ulog_topics"]
 
@@ -155,9 +156,23 @@ def compute_resampled_dataframe(self, ulog):
                 assert (len(topic_dict["dataframe_name"]) == len(topic_dict["ulog_name"])), (
                     'could not rename topics of type', topic_type, "due to rename list not having an entry for every topic.")
                 curr_df.columns = topic_dict["dataframe_name"]
-            df_list.append(curr_df)
             topic_type_bar.next()
+            if (topic_type == "vehicle_angular_velocity" and self.estimate_angular_acceleration):
+                print(curr_df)
+                ang_vel_np = curr_df[["ang_vel_x",
+                                      "ang_vel_y",  "ang_vel_z"]].to_numpy()
+                time_in_secods_np = (curr_df[["timestamp"]].to_numpy()/1000000)
+                time_in_secods_np = time_in_secods_np.flatten()
+                print(ang_vel_np)
+                ang_acc_np = np.gradient(ang_vel_np, time_in_secods_np, axis=0)
+                print(ang_acc_np)
+                topic_type_bar.next()
+                curr_df[["ang_acc_b_x", "ang_acc_b_y",
+                         "ang_acc_b_z"]] = ang_acc_np
+            df_list.append(curr_df)
+
         topic_type_bar.finish()
+        print(df_list)
         resampled_df = resample_dataframe_list(
             df_list, fts, self.resample_freq)
         return resampled_df.dropna()