Minor changes to yaml files

yaml/borinot_platform_simulation.yaml

 platform_params:
   cf: 4.511e-06 # cf computed with 60% thrust values (1196g at 15400rpm)
   cm: 2.706e-07
+  torque_correction: 0 # No need for correction in simulation
   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)

yaml/general_estimation_simulation.yaml

+config:
+
+  debug:
+    profiling: true
+    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,2.5] # 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:                .01            # std dev of acc noise in m/s2 # 0.6/4
+    gyro_noise_std:               .01           # std dev of gyro noise in rad/s 
+    accb_initial_std:             0.5           # m/s2    - initial bias
+    gyrob_initial_std:            0.1           # rad/sec - initial bias
+    acc_drift_std:                0.1           # std dev of acc drift m/s2/sqrt(s)
+    gyro_drift_std:               0.1           # std dev of gyro drift rad/s/sqrt(s)
+    bias:                         [0,0,0,0,0,0] # bias prior, usefull for fixing the bias when known
+    imu_bias_fix:                 true
+    
+    #FT
+    force_noise_std:              1      # std dev of force noise in N 
+    torque_noise_std:             1      # std dev of torque noise in N/m
+    
+    # Dynamics
+    com:                          [0.00,0.00,0.0]                      # center of mass [m] 
+    inertia:                      [0.014958,0.014648,0.0237319]      # moments of inertia i_xx, i_yy, i_zz [kg m2]
+    mass:                         2.02                                  # mass [kg]
+    com_fix:                      false
+    inertia_fix:                  false
+    mass_fix:                     false
+
+    # priors
+    set_mass_prior:               true
+    prior_mass_std:               1
+    set_inertia_prior:            true
+    prior_inertia_std:            [0.1,0.1,0.1]
+    set_com_prior:                true
+    prior_com_std:                [0.1,0.1,0.1] 
+    set_bias_prior:               true
+    prior_bias:                   [0,0,0,0,0,0]
+    prior_bias_std:               [0.1,0.1,0.1,0.1,0.1,0.1] 
+   -
+    name: "sensor Pose"
+    type: "SensorPose"
+    plugin: "core"
+    extrinsic:
+      pose: [0.0,0.0,0.0, 0,0,0,1] #amg
+    std_p: 0.001
+    std_o: 0.001
+
+
+  processors:
+   -
+    name: "proc FTID"
+    type: "ProcessorForceTorqueInertialDynamics"
+    sensor_name: "sensor FTI"
+    plugin: "bodydynamics"
+    time_tolerance: 0.01 # amg 1/50/2
+    apply_loss_function: false
+    unmeasured_perturbation_std: 0.00001
+    state_getter: true
+    state_priority: 1
+    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_sensorCombined: "/fmu/sensor_combined/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: 100
+      w_max: 2216
+      # w_max: 1116
+      quadratic_aprox: true # true in simulation
+      follow: "./borinot_platform_simulation.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.05
+      viz_scale: 0.3
+      text_scale: 0.3
+      landmark_text_z_offset: 0.3
+      landmark_length: 0.5
+      frame_width: 0.05
+      frame_length: 0.5
+      frame_vel_scale: 1.0
+      factors_absolute_height: 00.1
+    -
+      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
+

yaml/inertia_estimation_simulation.yaml

@@ -58,6 +58,7 @@ config:
     gyrob_initial_std:            0.1       # rad/sec - initial bias
     acc_drift_std:                0.1     # std dev of acc drift m/s2/sqrt(s)
     gyro_drift_std:               0.1     # std dev of gyro drift rad/s/sqrt(s)
+    bias:                         [0,0,0,0,0,0] # bias prior, usefull for fixing the bias when known
     imu_bias_fix:                 false
     
     #FT
@@ -65,10 +66,9 @@ config:
     torque_noise_std:             1      # std dev of torque noise in N/m
     
     # Dynamics
-    com:                          [0.00,0.00,0.0]                      # center of mass [m] 
-    inertia:                      [0.014958,0.014648,0.0237319]      # moments of inertia i_xx, i_yy, i_zz [kg m2]
-    # inertia:                      [0.015,0.015,0.015]
-    mass:                         2.02                                  # mass [kg]
+    com:                          [0.00,0.00,0.0]                  # center of mass [m] 
+    inertia:                      [0.014958,0.014648,0.0237319]    # moments of inertia i_xx, i_yy, i_zz [kg m2]
+    mass:                         2.02                             # mass [kg]
     com_fix:                      true
     inertia_fix:                  false
     mass_fix:                     true
@@ -133,6 +133,7 @@ config:
       topic: " "
       topic_acc: "/fmu/sensor_accel/out"
       topic_gyro: "/fmu/sensor_gyro/out"
+      topic_sensorCombined: "/fmu/sensor_combined/out"
       topic_actuators: "/fmu/actuator_outputs/out"
       topic_batteryStatus: "/fmu/battery_status/out"
       sensor_name: "sensor FTI"
@@ -178,4 +179,4 @@ config:
       map_frame_id: "map"
       odom_frame_id: "odom"
       base_frame_id: "base_footprint"
-      publish_odom_tf: true
+      publish_odom_tf: true
\ No newline at end of file

yaml/mass_and_com_estimation_simulation.yaml

@@ -52,12 +52,13 @@ config:
       pose: [0,0,0, 0,0,0,1]
     
     # IMU
-    acc_noise_std:                0.1      # std dev of acc noise in m/s2 # 0.6/4
-    gyro_noise_std:               0.01       # 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)
+    acc_noise_std:                0.1           # std dev of acc noise in m/s2 # 0.6/4
+    gyro_noise_std:               0.01          # 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)
+    bias:                         [0,0,0,0,0,0] # bias prior, usefull for fixing the bias when known
     imu_bias_fix:                 false
     
     #FT
@@ -68,6 +69,7 @@ config:
     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:                         2                                    # mass [kg]
+
     com_fix:                      false
     inertia_fix:                  true
     mass_fix:                     false 
@@ -87,7 +89,7 @@ config:
     type: "SensorPose"
     plugin: "core"
     extrinsic:
-      pose: [0.0,0.0,0.0, 0,0,0,1] #amg
+      pose: [0.0,0.0,0.0, 0,0,0,1] 
     std_p: 0.0001
     std_o: 0.0001
 
@@ -98,13 +100,11 @@ config:
     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   
@@ -131,12 +131,13 @@ config:
       topic: " "
       topic_acc: "/fmu/sensor_accel/out"
       topic_gyro: "/fmu/sensor_gyro/out"
+      topic_sensorCombined: "/fmu/sensor_combined/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
+      imu_x_axis: 1     # PX4 axis are frd (front right down)
+      imu_y_axis: -2    # PX4 axis are frd
+      imu_z_axis: -3    # PX4 axis are frd
       cov_imu_source: "sensor"
       in_degrees: false
       synchronization_mode: "imu"