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
- propagation sensor input for internal state propagation
- input from external state propagation (e.g. asctec_mav_framework)
Published Topics
state_out (ssf_core/DoubleArrayStamped)- all state information
- correction for external state propagation (e.g. asctec_mav_framework)
- 6DoF pose for underlying pose controller (e.g. asctec_mav_framework)
- 6DoF pose and velocity for underlying pose and velocity controller (e.g. asctec_mav_framework)
position_sensor
example of a 3DoF position sensor with scaled position measurementSubscribed Topics
position_measurement (ssf_updates/PositionWithCovarianceStamped)- the sensor's information source
- propagation sensor input for internal state propagation
- input from external state propagation (e.g. asctec_mav_framework)
Published Topics
state_out (ssf_core/DoubleArrayStamped)- all state information
- correction for external state propagation (e.g. asctec_mav_framework)
- 6DoF pose for underlying pose controller (e.g. asctec_mav_framework)
- 6DoF pose and velocity for underlying pose and velocity controller (e.g. asctec_mav_framework)
