mass and com estimation from simulated data

launch/mass_and_com_estimation_simulation.launch

+<!-- -->
+<launch>
+    <!--USER ARGS-->
+    <arg name="rviz" default="true" /> 
+    <arg name="speed" default="1" />
+    <arg name="sec" default="0" />
+    <arg name="sim_time" default="true" />
+    
+    <arg name="profiling" default="false" />
+    <arg name="gdb" default="false" />
+    <arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
+    <arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
+    <arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
+
+    <arg name="bag" default="rosbag_borinot_sim_hover_pure"/>
+    
+    
+    <!--VISUALIZATION-->
+    <group if="$(arg rviz)">
+        <node name="rviz"
+             pkg="rviz"
+             type="rviz" 
+             args="-d $(find wolf_demo_uav_identification)/rviz/online.rviz" />
+    </group>
+
+
+
+    <!--WOLF-->
+    <node type="wolf_ros_node" 
+          name="wolf_ros_node" 
+          pkg="wolf_ros_node"
+          output="screen"
+          required="true"
+          launch-prefix="$(arg launch_pref)">
+        <param name="~yaml_file_path" value="$(find wolf_demo_uav_identification)/yaml/mass_and_com_estimation_simulation.yaml" />
+        <remap from="/wrench_accel" to="/fmu/sensor_accel/out"/>
+        <remap from="/wrench_gyro" to="/fmu/sensor_gyro/out"/>
+        <remap from="/wrench_actuators" to="/fmu/actuator_outputs/out"/>
+    </node>
+
+    <!--ROSBAG-->
+    <node pkg="rosbag"
+        type="play"
+        name="player"
+        required="true"
+        args="-r $(arg speed)
+        -s $(arg sec)
+        --clock
+        $(find wolf_demo_uav_identification)/bag/$(arg bag).bag"/>
+    
+</launch>

yaml/borinot_platform.yaml

 platform_params:
-  # cf: 4.511e-06 # cf computed with 60% thrust values (1196g at 15400rpm)
-  cf: 7.7e-06 
-  # cm: 6.991478005829954e-08
-  cm: 4.5e-08
+  cf: 4.511e-06 # cf computed with 60% thrust values (1196g at 15400rpm)
+  # cf: 7.7e-06 
+  # cf: 1.2113e-06
+  cm: 6.991478005829954e-08
+  # cm: 4.5e-08
+  # cm: 2.706e-7
   max_thrust: 10 # Thrust limited to 10 N. Max available: 20.6991
   min_thrust: 0.0
   max_prop_speed: 2236.0 # Corresponding to its max (21350 rpm)
@@ -32,4 +34,15 @@ platform_params:
 
     - translation: [-0.1602147, 0.0925, 0.0]
       orientation: [0, 0, 0.965926, 0.258819]
-      spin_direction: [1]
\ No newline at end of file
+      spin_direction: [1]
+  thrust_coeff: # opt 1
+    k_const: 1
+    k_d1: 1
+    k_d2: 1
+    k_d3: 0
+    k_v1: 0
+    k_v2: 0
+    k_v3: 0
+    k_d1v1: 0
+    k_d2v1: 0
+    k_d1v2: 0

yaml/mass_and_com_estimation_simulation.yaml

+config:
+
+  debug:
+    profiling: false
+    profiling_file: "~/wolf_demo_uav_identification.txt"
+    print_problem: true
+    print_depth: 1          # only if print_problem
+    print_constr_by: false     # only if print_problem
+    print_metric: true        # only if print_problem
+    print_state_blocks: true  # only if print_problem
+    print_period: 3           # only if print_problem
+
+
+
+
+  problem:
+    node_rate: 100
+    frame_structure: "POVL"
+    dimension: 3
+    prior:
+      mode: "initial_guess"  ##
+      $state:
+        P: [0,0,10] # amg # initial position
+        O: [0,0,0,1] # amg # initial orientation
+        V: [0,0,0]
+        L: [0,0,0]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+        V: [100,100,100]
+        L: [100,100,100]
+      time_tolerance: 0.005
+    tree_manager: 
+      type: "TreeManagerSlidingWindow"
+      plugin: "core"
+      n_frames: 1000
+      n_fix_first_frames: 0
+      viral_remove_empty_parent: true
+  map:
+    type: "MapBase"
+    plugin: "core"
+
+  solver:
+    follow: "solver.yaml"
+
+  sensors:
+   -
+    name: "sensor FTI"
+    type: "SensorForceTorqueInertial"
+    plugin: "bodydynamics"
+    extrinsic:
+      pose: [0,0,0, 0,0,0,1]
+    
+    # IMU
+    acc_noise_std:                0.015      # std dev of acc noise in m/s2 # 0.6/4
+    gyro_noise_std:               0.05       # std dev of gyro noise in rad/s 
+    accb_initial_std:             0.01       # m/s2    - initial bias
+    gyrob_initial_std:            0.01       # rad/sec - initial bias
+    acc_drift_std:                0.0001     # std dev of acc drift m/s2/sqrt(s)
+    gyro_drift_std:               0.0001     # std dev of gyro drift rad/s/sqrt(s)
+    imu_bias_fix:                 false
+    
+    #FT
+    force_noise_std:              0.075      # std dev of force noise in N 
+    torque_noise_std:             0.01       # std dev of torque noise in N/m
+    
+    # Dynamics
+    com:                          [0.00,0.00,0.0]                      # center of mass [m] 
+    inertia:                      [0.0134943,0.0141622,0.0237319]      # moments of inertia i_xx, i_yy, i_zz [kg m2]
+    mass:                         1.5                                  # mass [kg]
+    com_fix:                      false
+    inertia_fix:                  true
+    mass_fix:                     false 
+
+   -
+    name: "sensor Pose"
+    type: "SensorPose"
+    plugin: "core"
+    extrinsic:
+      pose: [0.0,0.0,0.0, 0,0,0,1] #amg
+    std_p: 0.0001
+    std_o: 0.0001
+
+
+  processors:
+   -
+    name: "proc FTID"
+    type: "ProcessorForceTorqueInertialDynamics"
+    sensor_name: "sensor FTI"
+    plugin: "bodydynamics"
+    # time_tolerance: 0.00125 # 1/471/2
+    time_tolerance: 0.01 # amg 1/50/2
+    apply_loss_function: false
+    unmeasured_perturbation_std: 0.00001
+    state_getter: true
+    state_priority: 1
+    # n_propellers: 6
+    keyframe_vote:
+      voting_active:    true
+      max_time_span:    0.2  # De moment el que funciona millor és 1 s o 2s
+      max_buff_length:  999   # motion deltas
+      dist_traveled:    999   # meters
+      angle_turned:     999   # radians (1 rad approx 57 deg, approx 60 deg)
+
+   - 
+    name: "processor Pose"
+    type: "ProcessorPose"
+    sensor_name: "sensor Pose"
+    plugin: "core"
+    time_tolerance: 0.01 # 1 / 100Hz / 2 = 0.005
+    apply_loss_function: false
+    keyframe_vote:
+      voting_active: false
+
+
+
+  ROS subscriber:
+    -
+      package: "wolf_ros_bodydynamics"
+      type: "SubscriberForceTorqueInertialDrone"
+      topic: " "
+      topic_acc: "/fmu/sensor_accel/out"
+      topic_gyro: "/fmu/sensor_gyro/out"
+      topic_actuators: "/fmu/actuator_outputs/out"
+      topic_batteryStatus: "/fmu/battery_status/out"
+      sensor_name: "sensor FTI"
+      imu_x_axis: 1
+      imu_y_axis: -2
+      imu_z_axis: -3
+      cov_imu_source: "sensor"
+      in_degrees: false
+      synchronization_mode: "imu"
+      duty_min: 1000
+      duty_max: 2000
+      w_min: 0
+      w_max: 1116
+      # w_max: 1116
+      quadratic_aprox: true ######################################
+      follow: "./borinot_platform.yaml"
+    -
+      package: "wolf_ros_node"
+      type: "SubscriberPose"
+      topic: "/pose_from_tf" # amg # msg generated from tf to mimic the optitrack msg
+      sensor_name: "sensor Pose"
+
+
+  ROS publisher:
+    -
+      type: "PublisherGraph"
+      topic: "graph"
+      package: "wolf_ros_node"
+      period: 0.1
+      viz_scale: 0.3
+      text_scale: 0.3
+      landmark_text_z_offset: 0.3
+      landmark_length: 0.5
+      frame_width: 0.01
+      frame_length: 0.1
+      frame_vel_scale: 1.0
+    -
+      type: "PublisherTf"
+      topic: " "
+      package: "wolf_ros_node"
+      period: 0.1
+      map_frame_id: "map"
+      odom_frame_id: "odom"
+      base_frame_id: "base_footprint"
+      publish_odom_tf: true