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

Tutorial for Custom Sensor Module Design

Description: This tutorial shows how to design an update-sensor module for a custom sensor. You will learn how to implement the necessary routines as well as configure noise and delay parameters correctly.

Tutorial Level: ADVANCED

Next Tutorial: sfly_framework

Overview and Preparation

From the previous tutorial you are familiar with the structure of the ethzasl_sensor_fusion framework and know that for every different update sensor we need to design:

• a sensor-value acquisition-routine (i.e. measurement callback)
• an initialization routine (initializing the state vector according to the information currently available from the update sensor)

• the linearized measurement Matrix (H), the computation of the residuals (y) and the measurement-covariance matrix (R).

Once we designed our new update-sensor module, we will discuss how to set the values of the noise and delay parameters.

In this tutorial, we will design an update-sensor module for a 6DoF pose update-sensor from scratch. Such a sensor could be a single camera used in a visual odometry framework as in ethzasl_ptam or - as it will be in our case - the 6DoF pose measurement of a Vicon system. Note that the provided files pose_sensor.cpp, pose_sensor.h and pose_measurements.h already represent such a module for a 6DoF pose sensor - you may use these files as support in this tutorial.

We use the same dataset (3.7MB) as in the previous tutorial. This time, we will use the full 6DoF measurement of the Vicon.

Designing a 6DoF Vicon Pose Update-Sensor Module

A skeleton of the three module files can be downloaded here:

• template_sensor.h: The sensor header defining the member variables to store the sensor readings and noise values and the measurement callback function.

• template_sensor.cpp: The sensor body defining the computation of the measurement matrix H, the residuals y, the measurement covariance matrix R and the interaction with the dynamic parameters.

• template_measurements.h: The measurement sensor "host". Invokes the *_sensor class and defines the initialization routine.

Download the files here, copy them to ethzasl_sensor_fusion/ssf_updates/src and rename them from template_* to viconpose_*. As discussed previously this name declares the type of the update-sensor module and, thus, the type of the whole filter-framework.

Designing the Sensor-Header: *_sensor.h

First, open the file viconpose_sensor.h and adapt the template entries to viconpose accordingly (you may chose any suitable name for your sensor):

Then, we define all member variables we need to store the measured sensor values and the sensor-noise values. We will measure a 6DoF, that is, we need position and attitude. We denote measurement related variables with z, positions with p, attitudes with q (since we use quaternions) and noise values with n. Thus, for position and attitude measurements and noise values we use:

Eigen::Matrix<double, 3, 1> z_p_; /// sensor-position measurement
Eigen::Quaternion<double> z_q_;   /// sensor-attitude measurement
double n_zp_;                     /// noise for position measurement
double n_zq_;                     /// noise for attitude measurement

for our 6DoF pose sensor, this is already done in the template. Change it if you have a sensor that measures other values. (e.g. z_bv and n_zbv for a sensor that measures body velocities as explained in this paper).

Next, we need to define which ROS message type is used in the measurement callback-function. In our case, this will be the 6DoF pose from the Vicon packed into a geometry_msgs::TransformStamped message:

void measurementCallback(const geometry_msgs::TransformStampedConstPtr & msg);

do not forget to include the corresponding header file: #include <geometry_msgs/TransformStamped.h>.

With this, we defined the sensor header already.

Designing the Sensor-Body: *_sensor.cpp

Here we define the computation of the measurement matrix H, the residuals y, the measurement covariance-matrix R and the interaction with the dynamic parameters. This is the main part of an update-sensor module.

Again, we start by renaming the sensor type from template to viconpose. Open the file viconpose_sensor.cpp and adapt the template entries to viconpose accordingly (you may chose any suitable name for your sensor):

Then, we handle the different parameters from the reconfigure gui. In the subscribe() routine, the sensor registers the reconfigure callback noiseConfig(). The reconfigure gui provides two generic noise parameters meas_noise1 and meas_noise2. If the sensor does not include a covariance for its measurements in the ROS message, you may use these dynamic values to set a fix covariance calculated by you (see noise value determination at the end of this tutorial). In our case, the Vicon TransformStamped message does not contain a covariance information and we use ·meas_noise1 for the position measurement noise n_zp_ and meas_noise2 for the attitude measurement noise n_zq_. The variables store standard deviations.
Change the default values in the subscribe() routine to 0.005 (5 millimeter) and 0.01 (0.01 radians, about 0.5 degree) for position and attitude measurement noise respectively.
In noiseConfig() we make sure that the new noise values are stored in the foreseen member variables if they are changed in the dynamic reconfigure gui.

The big work is in designing the measurementCallback() function. Before staring, we copy the function header from the viconpose_sensor.h file.

void ViconPoseSensorHandler::measurementCallback(const geometry_msgs::TransformStampedConstPtr & msg)

First, we read the sensor values to the prepared member variables:

// get measurements     
z_p_ = Eigen::Matrix<double,3,1>(msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z);
z_q_ = Eigen::Quaternion<double>(msg->transform.rotation.w, msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z);

At this stage it makes sense to define the size (i.e. number) of the measurements. We can distinguish between sensor measurements and artificial measurements:

• sensor measurements: are simply the values the update-sensor provides (position and attitude in our vicon-pose update-sensor example).

• artificial measurements: with these constraints (rather than saying "measurements") we force the unobservable states of the system so some constant value. Depending on the used update-sensor different states used in ssf_core are unobservable (for example the attitude between the sensor and the IMU when using a 3D global position sensor as seen in the previous tutorial). Since these states are unobservable, the constraint imposed does not affect the filter performance. A thorough discussion on which states are observable with which sensor type can be found in PhD Thesis in chapter 3 (in particular Figure 3.12).
  

In our vicon-pose update-sensor example we have 6 sensor measurements (for the 6DoF vicon pose) and 1 artificial measurement for the yaw drift between the frame in which the sensor perceives its pose and a world fix reference frame (i.e. navigation frame). As we will see later in this tutorial, we simply align the yaw of both frames. This simply means that we let the "fix" world frame drift along the other reference frame. Of course, in this example the Vicon reference frame does not drift, hence the world frame will stay fix.
All in all, we have 7 measurements, hence we set in viconpose_sensor.cpp

#define N_MEAS 7

Next, we construct the measurement noise matrix R. Ideally, the sensor sends the values together with a covariance in the ROS message. In that case, the covariance is simply copied to R. Make sure, the parameter use_fixed_covariance is set to False. Care has to be taken on additional artificial measurements discussed before and in the previous tutorial and as we will see in the following.
In our case, we have no covariance information in the sensor message and we construct a diagonal matrix based on the noise standard deviations set in the dynamic reconfigure gui. In this case, make sure, the parameter use_fixed_covariance is set to True. From the dynamic reconfigure gui we take the noise values for position and attitude. Furthermore, we add a small noise value (10^-6) for the artificial measurement:

//  alternatively take fix covariance from reconfigure GUI
if (use_fixed_covariance_)
{
  const double s_zp = n_zp_ * n_zp_;
  const double s_zq = n_zq_ * n_zq_;
  R = (Eigen::Matrix<double, N_MEAS, 1>() << s_zp, s_zp, s_zp, s_zq, s_zq, s_zq, 1e-6).finished().asDiagonal();
}

We reserved the measurement_world_sensor parameter to identify if the measurement is taken as one seen from the sensor itself or from its reference frame. You need to take this into account later when defining the measurement matrix H and the residuals y. In this tutorial - as we use a Vicon pose - we assume the sensor measurement to be one that sees the sensor w.r.t. the reference frame. Hence we set the parameter measurement_world_sensor: True. Depending on your setup, it may be convenient to transform the sensor readings and covariance matrix from the sensor frame to the world frame or vice-a-versa. An example of such a transformation can be found in the template file template_sensor.cpp.

Once the measurements are captured (and maybe transformed) we store them to have the most recent values available for an eventual initialization call.

measurements->p_vc_ = z_p_;
measurements->q_cv_ = z_q_;

Since the linearization of the measurement matrix depends on the current state, we need to find the state which is temporarily closest to the current measurement (as explained in the ICRA12 paper this is the time delay compensation). This is already done for you in the template_sensor.cpp file.

The next step - computing the measurement matrix H - involves quite a bit of theory. This is beyond this tutorial and we strongly suggest to read chapter 3.1.1 of this PhD Thesis or at least the ICRA11 paper.
In this tutorial we have a position and attitude measurement from the Vicon and one artificial measurement for the yaw between Vicon frame and world (navigation) frame. Hence the H matrix is 7x25 (we have 25 states as explained in the previous tutorial). The measurement equations are:

z'_p = C_w^vT * (p + C_q^T * p_cⁱ) * λ
z'_q = q_w^v * q * q_cⁱ
z'q_v^w(yaw) = -2 * (q_v^w.w * q_v^w.z + q_v^w.x * q_v^w.y) / (1 - 2 * (q_v^w.y * q_v^w.y + q_v^w.z * q_v^w.z))

whereas the last equation is simply the extraction of the yaw angle from a quaternion And C_i^j is the rotation matrix of the quaternion q_i^j.
Linearizing these equations using the error state representation as described in the PhD Thesis, we obtain the H-block for the position measurement:

H_old.block<3, 3> (0, 0) = C_wv.transpose() * state_old.L_; // p
H_old.block<3, 3> (0, 6) = -C_wv.transpose() * C_q.transpose() * pci_sk * state_old.L_; // q
H_old.block<3, 1> (0, 15) = C_wv.transpose() * C_q.transpose() * state_old.p_ci_ + C_wv.transpose() * state_old.p_; // L
H_old.block<3, 3> (0, 16) = -C_wv.transpose() * skewold; // q_wv
H_old.block<3, 3> (0, 22) = C_wv.transpose() * C_q.transpose() * state_old.L_; //p_ci

the H-block for the attitude measurement:

H_old.block<3, 3> (3, 6) = C_ci; // q
H_old.block<3, 3> (3, 16) = C_ci * C_q; // q_wv
H_old.block<3, 3> (3, 19) = Eigen::Matrix<double, 3, 3>::Identity(); //q_ci

and the artificial measurement to keep the yaw of the Vicon reference frame aligned with the world frame:

H_old(6, 18) = 1.0;

we defined first the intermediate results:

  // preprocess for elements in H matrix
  Eigen::Matrix<double, 3, 1> vecold;
  vecold = (state_old.p_ + C_q.transpose() * state_old.p_ci_) * state_old.L_;
  Eigen::Matrix<double, 3, 3> skewold = skew(vecold);
  Eigen::Matrix<double, 3, 3> pci_sk = skew(state_old.p_ci_);

For calculating the residuals we use the difference between the estimated values z'_p, z'_q, z'_p, zq'_v^w(yaw) and the measured values. For the position error we obtain:

r_old.block<3, 1> (0, 0) = z_p_ - C_wv.transpose() * (state_old.p_ + C_q.transpose() * state_old.p_ci_) * state_old.L_;

for the attitude error we have:

Eigen::Quaternion<double> q_err = (state_old.q_wv_ * state_old.q_ * state_old.q_ci_).conjugate() * z_q_;
r_old.block<3, 1> (3, 0) = q_err.vec() / q_err.w() * 2; // convert to error state

and for the yaw alignment we have

r_old(6, 0) = -2 * (q_err.w() * state_old.q_wv_.z() + state_old.q_wv_.x() * state_old.q_wv_.y()) / (1 - 2 * (state_old.q_wv_.y() * state_old.q_wv_.y() + state_old.q_wv_.z() * state_old.q_wv_.z()));

At this point we have prepared all necessary variables to call the general EKF update step. Remember to pass along the index of the state used to calculate the variables. This information is needed to propagate the state to the current time after the update in the past (c.f. ICRA12 paper).

measurements->ssf_core_.applyMeasurement(idx, H_old, r_old, R);

Designing the Initialization Routine: *_measurements.h

Also here, rename the sensor type from template to viconpose. Open the file viconpose_measurements.h and adapt the template entries to viconpose accordingly (you may chose any suitable name for your sensor):

The constructor reads the inter-sensor transformation configuration. Depending on your update-sensor used, you may not need the full transformation but only translation (for a position update-sensor) or rotation (for an angular update-sensor).

The initialization routine is called with the scale argument. In this PhD Thesis we highlight, that a correct scale initialization is crucial, hence we made this initialization parameter available in the dynamic reconfigure gui.

Have a look at the template file or at the provided pose_measurements.h or position_measurements.h files on how to initialize the states. Essentially, for this tutorial, you could copy the initialization routine from pose_measurements.h. You may use the default covariance matrix for initialization unless you have a good measure for these values on your own.

We may also check if we already received measurements from the update-sensor in order to initialized better states as the position and attitude:

// check if we have already input from the measurement sensor
if (p_vc_.norm() == 0)
  ROS_WARN_STREAM("No measurements received yet to initialize position - using [0 0 0]");
if ((q_cv_.norm() == 1) & (q_cv_.w() == 1))
  ROS_WARN_STREAM("No measurements received yet to initialize attitude - using [1 0 0 0]");

This concludes the design of the new viconpose_sensor module for ethzasl_sensor_fusion.

Writing the Parameter File and Launching the New Module

This is done in the same way as discussed in the previous tutorial. You may name the fix parameter file viconpose_sensor_fix.yaml. Set the inter-sensor calibration attitude to the identity quaternion and the distance to zero. You can use this dataset to test the performance of your new update-sensor module.

A Note on Noisy Things and the State Vector

We support two ideas with respect to noise values and the states in the state vector:

1. The EKF framework has no tuning parameters. All noise values are given by the sensor manufacturer or can be calculated. If an EKF does not work as expected, one should not tune the noise values but use the correct ones and find the error in another place.

2. One should use all observable states in the state vector. Once the framework works in a satisfying way, one can start to remove states and keep states like the inter-sensor transformation fix in order to reduce the state dimension.

Finding the correct noise values

Manufacturers usually state the noise characteristics in the data sheet. See in the PhD Thesis chapter 3.1.1. for a discussion on how to handle the continuous time sensor noise characteristics on such a data sheet. You should use these values and not tune the filter with the noise characteristics.

Using All Observable States

For a notion which states may be observable, see chapter 3.4.6 and in particular figure 3.12 of the PhD Thesis. Note that some states need excitation in order to converge. Make sure you have sufficient excitation in the datasets during your testing phase. Once the framework is working properly, you may remove or fix some of the states (e.g. inter sensor-calibration, scale, etc) in order to have less requirements on the excitation for state convergence.
For debugging purpose, the dynamic reconfigure gui provides the options to keep the scale, biases and inter-sensor calibration states fix. You can use this option to see how your filter performs under different conditions.