Feature Request: Estimating Sensor Biases

[skohlbr]

As briefly talked about at ROScon: In many Kalman-Filter based state estimators, sensor biases can be estimated to improve accuracy. This is commonly done for gyro and accelerometer biases in all kinds of estimators from those on small MAVs to those used in aircraft. A common approach is augmenting the state vector with the sensor biases. One example using this approach is the ethzasl_sensor_fusion framework.

To provide a motivation why this is relevant also for robot_localization: Let's say I have a diff drive robot that provides wheel odometry and IMU data. Linear and angular velocities from wheel odometry and angular rate from gyro are to be fused by robot_localization. The gyro will invariably exhibit a bias error making the integrated orientation from it drift over time (definition of bias error here). Once we have another source of orientation data (i.e. wheel odometry), the gyro bias can be estimated in the filter with properly set up covariances. When it is known the robot is standing still from wheel odometry, any reported angular rate from the gyro is due to bias which can now be estimated and corrected for in the filter. Once the vehicle gets moving again and wheel-based angular rates are off, rates from the gyro can be integrated with higher accuracy and precision, as the bias gets compensated. This is not possible in robot_localization so far.

[NikolausDemmel]

+1

[ayrton04]

Yes, this seems like a useful feature. My only reservation is that I've been trying to keep the filter(s) as generic as possible, and adding the gyro biases makes an assumption about the input data, in that we assume the user has an IMU with a gyro. Would it be possible to do this as a separate node that calculates the biases and then feeds them as a different kind of input to the filter? If not, and augmenting the state vector is really the best way to do it, then we'll go that route.

[skohlbr]

Yes, I expect this very much to be a non-trivial addition.

There are indeed approaches that try decoupling the bias estimation from the regular filter, as 6 more states (in the case of acc + gyro biases) mean non-negligible additional computational load. One such approach is described in this paper by Bando et. al.

I'm not sure if another node would be the way to go (as there could be synchronization issues between nodes), but a component decoupled from the main filter (maybe plugin-based?) in the main estimator process might be another option.

In an ideal world, incoming IMU messages would have a boolean matrix associated with them (defaulting to all false) that allows selecting for which axes biases are to be estimated.

[ayrton04]

Thanks for the reference! I'll look into this.

[ayrton04]

So I think what I'm going to do is first add support for different filters via pluginlib, and get rid of having a different node for each filter type. Then I'll add these variables to the state vector, but allow each filter to operate on any subset of the data it wants. This way, users who don't wish to incur the computational overhead of tracking accelerometer and gyro biases can stick with the filters that don't. I expect we'll end up with a bunch of different plugins, but that's really a good thing in the end.

[davidmball]

+1 for this feature and for pluginlib and for keeping it in the same node.

However, I am not confident about the Bando et al paper. Unless I am misunderstanding their contribution or what you want to achieve they seem to have selectively ignored the large body of work on separate-bias estimation since the 1960's by Friedland and others.

This paragraph (Haessig and Friedland, 1998) has a good summary (although the contribution of this paper isn't as relevant here) https://www.researchgate.net/profile/David_Haessig/publication/3023089_Separate-bias_estimation_with_reduced-order_Kalman_filters/links/5508b8dd0cf27e990e0cc3e9.pdf

"The separate-bias filter as originally given in [1] has been the focus of significant interest and activity. Scores of references to the original paper have appeared in the literature (see the reference list given in [2]). And as is often the case with techniques that attract significant use, there have been a number of alternate derivations and extensions. Mendel [3] in 1978, and Ignagni [4] in 1981 developed two different derivations of the separate-bias filter. In both, the bias terms are assumed to be unknown and constant. In recent years, efforts have focused on the ramifications of nonconstant bias terms, i.e., bias that vary as a random walk process, _ b = w, where w is a vector of zero mean, Gaussian, white noise processes. Ignagni was the first to consider this case in [5], where he develops a two-stage estimator that is similar to the original in form and complexity, but which is suboptimal. He points out, however, that in many applications the bias terms vary slowly so that the degree of suboptimality is insignificant or acceptable. Alouani et al. in [6] propose a different two-stage estimator which is optimal if an algebraic constraint on the bias and state process noise covariances is satisfied. However, since this constraint is almost never satisfied in practice, it was their conclusion that all practical two-stage estimators based on their proposed solution would be suboptimal. An optimal two-stage filter which does not impose a constraint on the process noise covariances was presented by Hseih et al. in [7]. The proposed filter is significantly more complex than that of the original. Hseih points out that the additional complexity and computational burden could be unwarranted in some cases. It was not stated as to whether the original advantages of the two-stage estimator—improved numerical conditioning and potentially reduced computational burden—remain intact in Hseih’s proposed filter. Nevertheless, the cost for achieving optimality appears to be the loss of at least some part of those original advantages."

I'm not an expert at this and I haven't look at your code implementation. However, something like this paper (Ignagni, 2000) could be a good place to start? In particular would this approach fit in nicely with your ekf and ukf implementations? Ignagni, M., "Optimal and suboptimal separate-bias Kalman estimators for a stochastic bias," in Automatic Control, IEEE Transactions on , vol.45, no.3, pp.547-551, Mar 2000 doi: 10.1109/9.847741 URL: http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.126.9606&rep=rep1&type=pdf

[davidmball]

And there is a complete solution of the separate-bias which they call two-stage here for an unscented kalman filter. http://serv.yanchick.org/Books/%D0%9C%D0%B0%D1%82%D0%B5%D1%80%D0%B8%D0%B0%D0%BB%D1%8B%20%D0%BA%D0%BE%D0%BD%D1%84%D0%B5%D1%80%D0%B5%D0%BD%D1%86%D0%B8%D0%B9/acc08/data/papers/0551.pdf

Although I can't comment on the quality and they seem to have only tested in simulation.

[miquelmarti]

+1 This would be a really nice enhancement!

[ayrton04]

Yes, agreed. I'd like to make the different filter types plugins so that, for example, people who don't need bias estimation don't have to pay the extra computational expense for it. Unfortunately, my design is such that a plugin-based model will be a major undertaking. I'm willing to make that change, but it's going to be a major break for anyone who has forked the current version.

[ayrton04]

Just want to make it clear that this is still a priority for me. For r_l, I think it will be easiest to side-step using pluginlib, implement a single filter_node, let users specify which filter they want to use via a parameter, and then just instantiate a different filter type in the node based on the parameter. Then the filter that includes bias estimation can be a separated from the current ones.

[thiagodefreitas]

+1 Any news on that?

[ayrton04]

Not really. Sorry, I've been swamped with non-r_l things for a long time. After the documentation update (in progress), it's my top priority, though. I'm hoping to have some time to take a crack at this in the autumn.

[PymZoR]

+1 That would really be a killer-feature. Any updates ?

[ayrton04]

Sorry, I don't know that this will see the light of day. In order to properly estimate the biases, we'd have to dynamically adjust the size of the state vector to track the biases. Since we can have as many sensors as we want, we'd have to be able to scale the state vector and covariance matrix to arbitrary sizes, and that's going to be a huge undertaking that I simply don't have the cycles for.

However, we (at Locus) are actively developing the successor to r_l, and we will support this feature. I'm hoping to have something usable by ROSCon, but it'll be dependent on other demands.

[PymZoR]

Thanks @ayrton04 for keeping us up to date, hope to see this successor soon !

Cheers

[jabobian]

Thanks @ayrton04 for updating this issue.

I wonder if the gyro bias issue mentioned by skohlbr is also related to modern IMU (with gyro and magnetic sensors). Such IMU can fuse the gyro and magnetic data to get a more stable angle output. The bias of gyro can be filtered in AHRS algorithms such as the following one: http://x-io.co.uk/open-source-ahrs-with-x-imu/ which says: The filter also incorporates magnetic distortion compensation to overcome soft-iron disturbances and gyroscope bias drift compensation.

However, the dependence on magnetic sensor to get absolute orientation (relative to the Earth) brings another problem, that is if the magnetic field disturbance lasts for a not-too-short period of time (for example the robot moves too close to a electromagnet), then there will be some absolute system bias (which keeps changing aong with the varying magnetic field disturbance). Moreover, one may have to re-calibrate the IMU (I guess the magnetic sensor may have some hysterisis to clear, but I really don't know).

In my point of view, the magnetic sensor output (fused with gyro) is relatively stable and needs no integration, its bias is a single variable (for planar mobile robots). I wonder during the localization stage, can we use the orientation obtained in AMCL (scan match) to calculate this bias, and then publish (IMU_theta_z + bias) rather than IMU_theta_z to robot_localization to get a correct estimation of robot_theta_z? Thanks.