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 constant, core, part is incorporated in this package ssf_core

Nodes

This package only creates a library used by the ssf_updates for the different update sensors.

EKF Core Framework

The idea of having a core framework is to have have all fix parts of an EKF filter encapsulated in one library. Since we assume to always have an IMU as core (propagation) sensor we can incorporate the entire EKF propagation steps in the core framework. This leaves only the measurements as variable parts. More precisely, only the calculation of the measurement matrix and the residuals vary between the different measurement sensor types. Their handling is again fix and is incorporated in this package.

States

With this framework we aim at a (a) pose estimator (b) for power-on-and-go systems (c) used in long-term missions.

This requires several states to be estimated:

1. position, attitude and velocity for an underlying pose and velocity controller. For MAV navigation, we suggest to use asctec_mav_framework on an AscTec multi-copter with ethzasl_ptam as update sensor for accurate pose control.

2. pose and velocity are corrupted by sensor misalignment and biases. To avoid tedious calibration procedure before each mission on each vehicle, we estimate these intra- and inter-sensor calibration states. That is: the acceleration and gyroscope biases as well as the transformation between an update sensor and the IMU are estimated online during the mission. This renders the vehicle truly power-on-and-go.

3. pose and velocity are also corrupted by the drift of a non-global update sensor (e.g. ethzasl_ptam using a camera). A misalignment to gravity due to attitude drift of such a sensor results in fatal consequences for airborne vehicles. Also - if a monocular update sensor is used - drift in the scaling of the update sensor leads the underlying position controller to instability. Because of these issues we estimate these states online during the mission. More precisely this is the (visual) scaling factor and the update sensor drift in roll and pitch with respect to the navigation frame. This ensures metric scaled and gravity aligned position and velocity in long-term missions.

Prediction: Internal and External

The computationally costly part in the prediction is the quaternion integration. Position and velocity are propagated using first order integration. On the remaining states we apply zero order holds since we assume zero mean Gaussian white noise as process noises.

The ssf_core package has a built-in propagation procedure. However, you may also use an external state propagation as provided in the asctec_mav_framework:

• imuCallback() with the sensor_msg::Imu message invokes the internal state propagation

• stateCallback() with the custom ssf_core::ext_ekf message uses the external state propagation. You can still force the internal state propagation by setting the fix parameter data_playback=True.

Setting the Noise Values

Generally the noise values accessible in the dynamic reconfigure GUI should not be tuning parameters. Please check the acceleration and gyroscope noises and their bias noises in the manufacturer's data sheet of your IMU. Furthermore the noise values of the remaining states should be kept to zero since they are not temporally drifting. Instead, if you work with a keyframe based visual odomtery as update sensor you may add uncertainty to the visual scale and visual drift the moment a new keyframe is created. This reflects the spatial nature of these state's uncertainty.

EKF Update

This package only contains the EKF prediction step, a shell for the EKF update procedure and the error detection described in this paper. The actual calculations for the update matrix and residuals are done in a module of the ssf_updates package.

Using This Package

Apart of setting the correct noise values, the user should not need to modify this package since it only contains the core consisting of the propagation using IMU data and the shell for the EKF update. All update sensor specific calculations are done in ssf_updates.

Scripts

There are several scripts to plot the different states. plot_[states] T plots the states [states] in an rxgraph with a time history of T seconds (without T the default time history is used).

Node Information

ssf_core

main framework for single sensor fusion: library used by ssf_updates.

Parameters

Dynamically Reconfigurable Parameters

See the dynamic_reconfigure package for details on dynamically reconfigurable parameters.

~init_filter (bool, default: False)

• call filter init using defined scale

~scale_init (double, default: 1.0)

• value for initial scale Range: 0.001 to 30.0

~fixed_scale (bool, default: False)

• fix scale

~fixed_bias (bool, default: False)

• fix biases

~fixed_calib (bool, default: False)

• fix calibration states

~noise_acc (double, default: 0.0083)

• noise accelerations (std. dev) Range: 0.0001 to 0.5

~noise_accbias (double, default: 0.00083)

• noise acceleration bias (std. dev) Range: 1e-07 to 0.1

~noise_gyr (double, default: 0.0013)

• noise gyros (std. dev) Range: 0.0001 to 0.5

~noise_gyrbias (double, default: 0.00013)

• noise gyro biases (std. dev) Range: 1e-07 to 0.1

~noise_scale (double, default: 0.0)

• noise scale (std. dev) Range: 0.0 to 10.0

~noise_qwv (double, default: 0.0)

• noise qwv (std. dev) Range: 0.0 to 10.0

~noise_qci (double, default: 0.0)

• noise qci (std. dev) Range: 0.0 to 10.0

~noise_pic (double, default: 0.0)

• noise pic (std. dev) Range: 0.0 to 10.0

~delay (double, default: 0.03)

• fix delay in seconds Range: -2.0 to 2.0

~set_height (bool, default: False)

• call filter init using defined height

~height (double, default: 1.0)

• height in m for init Range: 0.1 to 20.0

~meas_noise1 (double, default: 0.01)

• noise for measurement sensor (std. dev) Range: 0.0 to 10.0

~meas_noise2 (double, default: 0.01)

• noise for measurement sensor (std. dev) Range: 0.0 to 10.0

Static parameters

parameters that are statically set

~data_playback (, default: False)

• Flag to force internal state propagation even though the custom message ssf_core::ext_ekf is used as propagation sensor input