About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Only released in EOL distros:   fuerte groovy

Overview

The single sensor fusion (ssf) part of this stack is divided into the common Kalman Filter steps prediction (or propagation) and update (or measurement). We assume to always have an IMU as propagation sensor on the vehicle while we can change the update sensors depending on the users preferences. Hence, the propagation part remains unchanged for different update sensors. Essentially, for every different update sensor only the measurement matrix, and the calculation of the residuals change - the rest of the filter framework remains the same. This package contains samples for two different update sensors: a 6DoF pose sensor and a 3DoF position sensor.

Nodes

The provided nodes are only examples of update sensor modules. Since your hardware set-up and transformation conventions may differ from ours, before you use these examples you must check their code and adjust it to your needs.

pose_sensor: sample for a 6DoF pose sensor as update sensor (for example using Vicon or ethzasl_ptam)

position_sensor: sample for a 3DoF position sensor as update sensor (for example using a Leica Totalstation)

Designing an Update Sensor Module

In the following, we state the conceptual steps to design your own update sensor module. Please see the Tutorials to get a practical insight to the mentioned steps.

Every update sensor consists of 3 files:

• [type]_sensor.cpp and [type]_sensor.h contain the topic subscription, dynamic reconfigure calls and - most important - the calculation of the measurement matrix and residuals passed to ssf_core.

• [type]_measurements.h contains the initialization values passed to the initialization procedure in ssf_core.

Essentially you can use the example modules pose_sensor and position_sensor and adapt them to your needs.

The main.cpp file only provides the wrapper to instantiate a sensor module.

Subscribing to the Correct Topics

Each sensor module needs to subscribe to its own information source (e.g. Vicon bridge or output of ethzasl_ptam). The subscription to the propagation sensor (IMU) for external or internal state propagation is done automatically by ssf_core.

Registering Dynamic Reconfigure Calls

The dynamic reconfigure part in ssf_core contains two general noise parameters meas_noise1 and meas_noise2. They can be used as dynamically reconfigurable measurement noise parameters. However, we suggest to have the measurement noise included in the update sensor's own information source (e.g. covariance estimate of the visual odometry's PoseWithCovarianceStamped message).

Calculating the Measurement Matrix and Residuals

This is the mos crucial part and varies from sensor type to sensor type. Note that the same module can be used for a 6DoF Vicon information or for the 6DoF pose information from ethzasl_ptam. In the former information source the "visual" scale estimate should - of course - result in 1.0. However, attention must be paid to the definition of the 6DoF pose the information source is providing.

To calculate the measurement matrix and residuals one generally tries to express the measurements aid of the states. Please see the following papers to get some insights to this: pdf1, pdf2, pdf3

The code examples in pose_sensor and position_sensor already cover the two most important types of sensors and may be adapted to your needs.

Designing the Initialization

The correct initialization of the states is crucial since EKF frameworks may get stuck in local minima. We provide here a detailed overview on which states are more sensitive to correct initializations. The most delicate state is the position scaling factor (not more than 5% off the true value), hence we suggest to provide this via dynamic reconfigure GUI upon filter initialization. Using the first update sensor measurements together with this scaling factors some other states can be calculated. The remaining states are not very sensitive to correct initialization - an "eye based" initial value may be sufficient.

Node Information

pose_sensor

example of a 6DoF pose sensor with scaled position measurement (e.g. ethzasl_ptam)

Subscribed Topics

pose_measurement (geometry_msgs/PoseWithCovarianceStamped)

• the sensor's information source

imu_state_input (sensor_msgs/Imu)

• propagation sensor input for internal state propagation

hl_state_input (sensor_fusion_comm/ExtEkf)

• input from external state propagation (e.g. asctec_mav_framework)

Published Topics

state_out (sensor_fusion_comm/DoubleArrayStamped)

• all state information

correction (sensor_fusion_comm/ExtEkf)

• correction for external state propagation (e.g. asctec_mav_framework)

pose (geometry_msgs/PoseWithCovarianceStamped)

• 6DoF pose for underlying pose controller (e.g. asctec_mav_framework)

ext_state (sensor_fusion_comm/ExtState)

• 6DoF pose and velocity for underlying pose and velocity controller (e.g. asctec_mav_framework)

Parameters

Static parameters

parameters that are statically set

~init/q_ci/w (, default: 1.0)

• Initial value for the real part of the quaternion describing the rotation of the sensor (camera) w.r.t. the IMU, i.e. q_ci brings camera coordinates to IMU coordinates.

~init/q_ci/x (, default: 0.0)

• see above, imaginary part x

~init/q_ci/y (, default: 0.0)

• see above, imaginary part y

~init/q_ci/z (, default: 0.0)

• see above, imaginary part z

~init/p_ci/x (, default: 0.0)

• Initial value of the translation of the sensor (camera) w.r.t. the IMU, i.e. p_ci brings camera coordinates to IMU coordinates.

~init/p_ci/y (, default: 0.0)

• y

~init/p_ci/z (, default: 0.0)

• z

~use_fixed_covariance (, default: false)

• selects if covariance reported by the sensor is used or if fixed values from the reconfigure guy from ssf_core should be taken (meas_noise1 for position, meas_noise2 for attitude)

~measurement_world_sensor (, default: true)

• selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam)

position_sensor

example of a 3DoF position sensor with scaled position measurement

Subscribed Topics

position_measurement (ssf_updates/PositionWithCovarianceStamped)

• the sensor's information source

imu_state_input (sensor_msgs/Imu)

• propagation sensor input for internal state propagation

hl_state_input (sensor_fusion_comm/ExtEkf)

• input from external state propagation (e.g. asctec_mav_framework)

Published Topics

state_out (ssf_core/DoubleArrayStamped)

• all state information

correction (sensor_fusion_comm/ExtEkf)

• correction for external state propagation (e.g. asctec_mav_framework)

pose (geometry_msgs/PoseWithCovarianceStamped)

• 6DoF pose for underlying pose controller (e.g. asctec_mav_framework)

ext_state (sensor_fusion_comm/ExtState)

• 6DoF pose and velocity for underlying pose and velocity controller (e.g. asctec_mav_framework)