Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_uav_identification
1 result
Show changes
View open merge request
Commits on Source (19)
Hide whitespace changes
Inline Side-by-side
Showing
with 1844 additions and 16 deletions
.gitignore
Edit View file @ 0b464ab5
.settings
.vscode/
build/
bag/rosbag_full.bag
bag/rosbag_hovering.bag
bag/rosbag_yaw.bag
bag/*
\ No newline at end of file
README.md
Edit View file @ 0b464ab5
# wolf_demo_uav_identification
Check [WOLF documentation web](http://www.iri.upc.edu/wolf) (section WOLF Demos) for more information.
## Set up
### Wolf core, imu and bodydynamics installation
```
wget https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_scripts/-/raw/main/install_wolf.sh
sudo chmod +x install_wolf.sh
./install_wolf.sh [-d <string>] [-w <string>] [-a] -p "ynnnny"
```
Depencies
* wolf core - devel branch
* wolf imu - main
* wolf bodydynamics - identification branch
### Ros 1
The computer needs an operative [ros noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) installation.
### Wolf ros
``` Dependecies
wolf_ros_node - main
wolf_ros_imu - devel
wolf_ros_bodydynamics - identification
```
## Simulation
### Real values
* Mass: 2 Kg
* COM: [0, 0, 0] mm
* Inertia tensor diagonal of the base link: [0.0134943, 0.0141622, 0.0237319]
### Mass and com estimation
To estimate the mass and center of mass from hover rosbag in simulation run
```
roslaunch wolf_demo_uav_identification mass_and_com_estimation_simulation.launch bag:=rosbag_borinot_sim_hover_V2
```
*Note: From a hover rosbag it is impossible to estimate the z coordinate of the COM*
Estimation of the mass and center of mas from a general rosbag in simulation
```
roslaunch wolf_demo_uav_identification mass_and_com_estimation_simulation.launch bag:=rosbag_borinot_sim_general_V6
```
### Inertia estimation
#### YAW
Estimation of the Izz component of the inertia tensor from hover + yaw rosbag in simulation
```
roslaunch wolf_demo_uav_identification inertia_estimation_simulation.launch bag:=rosbag_borinot_sim_yaw_V2
```
*Note: Since no information on the Ixx and Iyy components is being injected from a hover + yaw rosbag it might be interesting to fix through hard normalization the Ixx and Iyy.*
#### All axis
To estimate the inertia tensor from a general movement rosbag run
```
roslaunch wolf_demo_uav_identification inertia_estimation_simulation.launch bag:=rosbag_borinot_sim_general_V6
```
### General estimation
Estimation of all dynamic parameters from a general movement rosbag run
```
roslaunch wolf_demo_uav_identification general_estimation_simulation.launch bag:=rosbag_borinot_sim_general_V6
```
*Note 1: This of course is the harder estimation for the solver. First, consider pausing the rosbag every second or so to give time for the solver to converge.*
*Note 2: Another idea is to fix the bias (either to 0 or to a value found on previous estimations e.g mass and com estimation). This reduces the number of variables to estimate.*
## Real
### Mass estimation
To estimate the mass from hover real rosbag run
```
roslaunch wolf_demo_uav_identification mass_estimation.launch bag:=rosbag_borinot_real_hover_V1_ok
```
### Mass and com estimation
To estimate the mass and center of mass from hover real rosbag run
```
roslaunch wolf_demo_uav_identification mass_and_com_estimation.launch bag:=rosbag_borinot_real_hover_V1_ok
```
*Note: From a hover rosbag it is impossible to estimate the z coordinate of the COM*
Estimation of the mass and center of mas from a general rosbag in simulation
```
roslaunch wolf_demo_uav_identification mass_and_com_estimation.launch bag:=rosbag_borinot_real_rpy_V2_ok
```
### Inertia estimation
To estimate the Izz inertia
```
roslaunch wolf_demo_uav_identification inertia_estimation.launch bag:=rosbag_borinot_real_rpy_V2_ok
```
*With regularization of [0.1,0.1,0.1] => ( 0.0228 0.0224 0.0295 )*
## Usefull stuff
1. Add ```| grep "Est, Sta"``` after the roslaunch command so only the estimated parameters will be shown (not the hole wolf tree).
2. In the case the solver does not solve the problem fast enough it is always an option to slow down the rosbag through the speed tag ```speed:=0.5``` (to reduce the speed a factor 0.5) in the roslaunch command
Check [WOLF documentation web](http://www.iri.upc.edu/wolf) (section WOLF Demos) for more information.
launch/Izz_estimation_simulation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_yaw_V2"/>
<!--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/inertia_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>
launch/com_estimation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_real_hover_V0_with_pose_nc"/>
<!--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/com_estimation.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>
launch/general_estimation_simulation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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)" />
<!--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/general_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>
launch/inertia_estimation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_real_rpy_V3_ok"/>
<!--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/inertia_estimation.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>
launch/inertia_estimation_simulation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_general_V7"/>
<!--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/inertia_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>
launch/mass_and_com_estimation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_real_rpy_V3_ok"/>
<!--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.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>
launch/mass_and_com_estimation_simulation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_V2"/>
<!--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>
launch/mass_estimation.launch 0 → 100644
Edit View file @ 0b464ab5
<!-- -->
<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_real_hover_V0_ok"/>
<!--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_estimation.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
Edit View file @ 0b464ab5
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: 3.847e-6
cm: 5.361e-8 # cm = 0.01393568 * 3.8e-4
torque_correction: 2.2e-4
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 +31,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: # constants for duties in range [-1,1]
k_const: 50.56872739133902
k_d1: -1.91832776
k_d2: 1.92498767
k_d3: -1.61925776
k_v1: -6.91528916
k_v2: 3.34151266e-01
k_v3: -4.97846198e-03
k_d1v1: 5.05657455e-01
k_d2v1: -7.27929315e-02
k_d1v2: -1.78239227e-03
yaml/borinot_platform_simulation.yaml 0 → 100644
Edit View file @ 0b464ab5
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)
min_prop_speed: 0.0
base_link_name: "borinot__base_link"
n_rotors: 6
$rotors:
- translation: [0.0, -0.185, 0.0]
orientation: [0, 0, -0.707107, 0.707107]
spin_direction: [1]
- translation: [0.0, 0.185, 0.0]
orientation: [0, 0, 0.707107, 0.707107]
spin_direction: [-1] # (-) means CCW
- translation: [0.1602147, 0.0925, 0.0]
orientation: [0, 0, 0.258819, 0.965926] # x, y, z, w
spin_direction: [1]
- translation: [-0.1602147, -0.0925, 0.0]
orientation: [0, 0, 0.965926, -0.258819]
spin_direction: [-1]
- translation: [0.1602147, -0.0925, 0.0]
orientation: [0, 0, -0.258819, 0.965926]
spin_direction: [-1]
- translation: [-0.1602147, 0.0925, 0.0]
orientation: [0, 0, 0.965926, 0.258819]
spin_direction: [1]
thrust_coeff: # calculated from the quadratic aproximation
k_const: 5.618252
k_d1: -0.0112365
k_d2: 5.61825e-6
k_d3: 0
k_v1: 0
k_v2: 0
k_v3: 0
k_d1v1: 0
k_d2v1: 0
k_d1v2: 0
yaml/com_estimation.yaml 0 → 100644
Edit View file @ 0b464ab5
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: [1, 1, 1]
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: 2 # std dev of acc noise in m/s2 # 0.6/4
gyro_noise_std: 0.1 # 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: 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.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: true
# regularization
set_com_prior: true
prior_com_std: [10,10,10]
set_inertia_prior: true
prior_inertia_std: [1,1,1]
set_mass_prior: true
prior_mass_std: 10
set_bias_prior: true
prior_bias_std: [10,10,10,10,10,10]
-
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.1
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: 100 # arm rotor velocity
w_max: 2216
quadratic_aprox: false # true in simulation
follow: "./borinot_platform.yaml"
-
package: "wolf_ros_node"
type: "SubscriberPose"
topic: "/pose_from_tf" # 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.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/general_estimation_simulation.yaml 0 → 100644
Edit View file @ 0b464ab5
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.yaml 0 → 100644
Edit View file @ 0b464ab5
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: 200
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: [1, 1, 1]
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.5 # std dev of acc noise in m/s2 # 0.6/4
gyro_noise_std: 0.03 # 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: 1 # std dev of force noise in N
torque_noise_std: 0.7 # std dev of torque noise in N/m
# Dynamics
com: [-0.000752,0.00178,-0.0148] # center of mass [m]
inertia: [0.017,0.017,0.029] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2.26 # mass [kg]
# bias: [-0.0416, 0.0308, 0.0967, 0.00203, -0.000916, 0.000327]
# bias: [0.012, -0.0138, -0.0353, -0.000621, -0.00566, -0.00305]
# 0.0129 -0.0137 -0.0227 0.000392 -0.00693 -0.00351
bias: [0,0,0,0,0,0]
com_fix: true
inertia_fix: false
mass_fix: true
# regularization
set_com_prior: true
prior_com_std: [0.5,0.5,0.5]
set_inertia_prior: true
prior_inertia_std: [0.1,0.1,0.1]
set_mass_prior: true
prior_mass_std: 10
set_bias_prior: true
prior_bias_std: [1,1,1,1,1,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.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.1
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 # arm rotor velocity
w_max: 2216
quadratic_aprox: false # true in simulation
follow: "./borinot_platform.yaml"
-
package: "wolf_ros_node"
type: "SubscriberPose"
topic: "/pose_from_tf" # 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.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 0 → 100644
Edit View file @ 0b464ab5
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,0] # amg # initial position
O: [0,0,0,1] # amg # initial orientation
V: [0,0,0]
L: [0,0,0]
$sigma:
P: [10, 10, 10]
O: [10, 10, 10]
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: false
#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: true
inertia_fix: false
mass_fix: true
# 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: [1,1,1]
set_bias_prior: true
prior_bias: [0,0,0,0,0,0]
# prior_bias: [0.0681, -0.0337, -0.123, 0.00258, -0.00129, -0.000407]
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]
std_p: 0.001
std_o: 0.001
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.5 # 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" # 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.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
\ No newline at end of file
yaml/mass_and_com_estimation.yaml 0 → 100644
Edit View file @ 0b464ab5
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: [1, 1, 1]
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: 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)
imu_bias_fix: false
#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.017,0.017,0.029] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2
bias: [0,0,0,0,0,0] # mass [kg]
com_fix: false
inertia_fix: true
mass_fix: false
# regularization
set_com_prior: true
prior_com_std: [0.5,0.5,0.5]
set_inertia_prior: true
prior_inertia_std: [1,1,1]
set_mass_prior: true
prior_mass_std: 10
set_bias_prior: true
prior_bias_std: [10,10,10,10,10,10]
-
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.1
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 # arm rotor velocity
w_max: 2216
quadratic_aprox: false # true in simulation
follow: "./borinot_platform.yaml"
-
package: "wolf_ros_node"
type: "SubscriberPose"
topic: "/pose_from_tf" # 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.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/mass_and_com_estimation_simulation.yaml 0 → 100644
Edit View file @ 0b464ab5
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.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
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.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
# regularization
set_com_prior: true
prior_com_std: [10,10,10]
set_inertia_prior: true
prior_inertia_std: [1,1,1]
set_mass_prior: true
prior_mass_std: 10
set_bias_prior: true
prior_bias_std: [10,10,10,10,10,10]
-
name: "sensor Pose"
type: "SensorPose"
plugin: "core"
extrinsic:
pose: [0.0,0.0,0.0, 0,0,0,1]
std_p: 0.0001
std_o: 0.0001
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
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 # 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"
duty_min: 1000
duty_max: 2000
w_min: 100 # arm rotor velocity
w_max: 2216
quadratic_aprox: true # true in simulation
follow: "./borinot_platform_simulation.yaml"
-
package: "wolf_ros_node"
type: "SubscriberPose"
topic: "/pose_from_tf" # 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.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/mass_estimation.yaml 0 → 100644
Edit View file @ 0b464ab5
config:
debug:
profiling: false
profiling_file: "~/wolf_demo_uav_identification.txt"
print_problem: true
print_depth: 2 # 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: [1, 1, 1]
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: 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]
imu_bias_fix: false
#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.0134943,0.0141622,0.0237319] # moments of inertia i_xx, i_yy, i_zz [kg m2]
mass: 2 # mass [kg]
com_fix: true
inertia_fix: true
mass_fix: false
# regularization
set_com_prior: true
prior_com_std: [0.5,0.5,0.5]
set_inertia_prior: true
prior_inertia_std: [1,1,1]
set_mass_prior: true
prior_mass_std: 10
set_bias_prior: true
prior_bias_std: [10,10,10,10,10,10]
-
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.1
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 # arm rotor velocity
w_max: 2216
quadratic_aprox: false # true in simulation
follow: "./borinot_platform.yaml"
-
package: "wolf_ros_node"
type: "SubscriberPose"
topic: "/pose_from_tf" # 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.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/solver.yaml
Edit View file @ 0b464ab5
minimizer: "LEVENBERG_MARQUARDT"
interrupt_on_problem_change: false
min_num_iterations: 4
max_num_iterations: 20
function_tolerance: 1e-7
gradient_tolerance: 1e-9
min_num_iterations: 5
max_num_iterations: 500
function_tolerance: 1e-10
gradient_tolerance: 1e-10
use_nonmonotonic_steps: false
period: 0.1 # note: this must be smaller (i.e. faster) than the KF period in processor_tracker_landmark_apriltag.yaml
n_threads: 2
period: 1 # note: this must be smaller (i.e. faster) than the KF period in processor_tracker_landmark_apriltag.yaml
n_threads: 8
compute_cov: false
verbose: 0
\ No newline at end of file
cov_enum: 1 # compute all marginals
cov_period: 1
verbose: 1
\ No newline at end of file