How to handle non-addable state variables?

[cboulay]

@HipsterSloth and I are working on a SRUKF for sensor fusion comprising a 9-DOF IMU and optical tracker. (It's for PC software using the playstation move controller). Our state-vector is 16-dimensionsal including

• position x/y/z
• velocity x/y/z
• acceleration x/y/z
• orientation (quat) w/x/y/z (4 variables but only 3 DOF)
• angular acceleration x/y/z (or yaw/pitch/roll, or p/q/r)

Unfortunately, quaternions cannot be added together. They must be multiplied together with special quaternion multiplication. This causes problems at a few steps of the (SR)UKF.

• mean +/- scaled_process_covariance during calculation of sigma-points
• Averaging propagated sigma points to get state prediction
• Difference between propagated sigmas and state mean to get new process covariance
• Same as above but when calculating cross-variance between state and observation for Kalman gain.

We can inherit from SquareRootUnscentedKalmanFilter and override the relevant functions, which will mostly be a re-write of what's already there except for the single problematic step, but I wonder if there's a more generally-useful solution.

• Instead of overriding the functions that contain the above steps, can those functions be re-written so that within they call another function to do these problematic steps?

For example, this

sigmaStatePoints.template block<State::RowsAtCompileTime, State::RowsAtCompileTime>(0,1)
                    = ( this->gamma * _S).colwise() + x;
// Set right block with x - gamma * S
sigmaStatePoints.template rightCols<State::RowsAtCompileTime>()
                    = (-this->gamma * _S).colwise() + x;

becomes

sigmaStatePoints.template block<State::RowsAtCompileTime, State::RowsAtCompileTime>(0,1)
                    = subtract(x, (-this->gamma * _S).colwise());
// Set right block with x - gamma * S
sigmaStatePoints.template rightCols<State::RowsAtCompileTime>()
                    = subtract(x, this->gamma * _S).colwise());

Edit to be clear: There would then be an overridable subtract() method.

Alternatively, is there a way to override the + and - operators on our state variables? Though this doesn't help with averaging so it's an incomplete solution.

While we're discussing generalizable solutions, we should also consider the possibility that someone will have something non-addable in their observation vector.

[mherb]

Hi @cboulay and @HipsterSloth! Sorry for getting back to you so late, I didn't really have time to spend on the library. You've probably solved your problem by now, but I still wanted to give you an update on this issue.

The challenge you're describing is that your state space partly lives on a manifold that is different from the euclidean space (quaternions on SO(3) in your case). A mathematically sound way to deal with such manifolds in state estimation problems is using "box" operators to modify elements on the manifold using small local adjustments with addition and subtraction operators. See (for instance) https://arxiv.org/abs/1107.1119 for details on this.

I've put support for manifold state spaces on the todo list (#18), but I can't tell when it will be added.

[jwdinius]

It is probably very late to be referencing this issue, but I have implemented a modification that adds function definitions for the predict and update methods that take a std::function instance as the final arg. These functions define a residual (a minus operator) for two objects in the state and measurement spaces, respectively. In this way, it is similar to what is done in the filterpy library. For UKF-style estimators, you could add a definition to the predict call for a state-mean calculation (your averaging, or plus, operator). This sounds more simple, and would merely extend rather than change the existing library (which I find to be great @mherb and thank you for it). If this sounds like something you would like in the baseline, I can submit an MR. Let me know.