diff --git a/yaml/borinot_platform_simulation.yaml b/yaml/borinot_platform_simulation.yaml
index a7ae69834e30123ade8c4d821d6b0b0ec5508212..4044503ebb52067a5dc8b893a758efb446115dec 100644
--- a/yaml/borinot_platform_simulation.yaml
+++ b/yaml/borinot_platform_simulation.yaml
@@ -1,6 +1,7 @@
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)
diff --git a/yaml/general_estimation_simulation.yaml b/yaml/general_estimation_simulation.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6bd2a74a2d98abb196a480b9a615696350cf8270
--- /dev/null
+++ b/yaml/general_estimation_simulation.yaml
@@ -0,0 +1,180 @@
+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
+
diff --git a/yaml/inertia_estimation_simulation.yaml b/yaml/inertia_estimation_simulation.yaml
index fdbbfe30a5b76e72dff85f2d0e75bdbcc77b6e29..f992b6df5bc4bf6698e89402b68093646db4c3b6 100644
--- a/yaml/inertia_estimation_simulation.yaml
+++ b/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
diff --git a/yaml/mass_and_com_estimation_simulation.yaml b/yaml/mass_and_com_estimation_simulation.yaml
index c463cb846fd3df127996542c9a3295ed82bdef7b..8f62229aa3076ce57f6af6ae6b2ab81866d71ae1 100644
--- a/yaml/mass_and_com_estimation_simulation.yaml
+++ b/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"