Does MeasurementVector support quaternion?

[xdotzv]

First thanks for your work.

I can not see any example using quaternion in measurement. And I tried myself

enum MyMeasurementFields
{
    SLAMPosition,
    SLAMAttitude
};
using MyMeasurementVector = UKF::DynamicMeasurementVector<
     UKF::Field<SLAMPosition, UKF::Vector<3>>
    ,UKF::Field<SLAMAttitude, UKF::Quaternion>
>;
// ...
           MyMeasurementVector m;
           m.set_field<SLAMPosition>(UKF::Vector<3>(t.x, t.y, t.z ));
           m.set_field<SLAMAttitude>(UKF::Quaternion(t.qw, t.qx, t.qy, t.qz));
           filter.innovation_step(m);
// ..

The above code get compilation error such as
binary '-': 'const Eigen::Quaternion<double,0>' does not define this operator or a conversion to a type acceptable to the predefined operator. (seems that measurement_delta does not have specialization for quaternion)

My question is:

1. Does measurementVector support quaternion ?
2. If no, can you provide any implementation hint ?

Thanks

[danieldyer]

Hi,

There's not support for quaternions in the MeasurementVector at the moment. I didn't add it since I didn't expect it to be a common use case – normally the attitude quaternion is in the state vector and is estimated using a number of sensors, each of which normally only has partial observability of attitude (magnetometers, thermopiles, accelerometer for inclination etc).

The only reason I can think of to have a quaternion in the measurement vector is if you're treating some external AHRS as a sensor, and fusing its attitude estimate with other sensors. Is that what you're trying to do?

[danieldyer]

As for implementation, it should be possible, but would likely require a bit of refactoring since adding and subtracting quaternions isn't going to give the right results. The right approach would probably be similar to how quaternion support is implemented in the StateVector class.

[xdotzv]

Thanks for your reply, I might give it a try.

And yes, that is what I want to do, fusing SLAM's position and attitude estimate with inertial sensor.

[Caiuse]

@xdotzv did you manage this? Please could you share some of your findings.

Would refactoring to allow for quarternion as a measurement vector be trivial or does it require a deep grasp on your UKF implementation? This looks like the perfect solution to my problem but unfortunately time isn't on my side.

[asymingt]

I can think of an example where quaternions support in the measurement vector would be useful. Consider a vision based tracking system in which a very efficient ePnP implementation can be used to estimate the pose of the camera in the world frame based on N correspondences between features in image plane and some sparse map that is known a priori. Here you have two options:

1. Use a measurement model that predicts the image-plane coordinate of all N features, resulting in a measurement vector of size 2N. Harder to implement as instrinsics/extrinsics have to be burnt into the filter, measurement update complexity scales with number of features.

2. Use a measurement model that predicts the camera pose in the world frame, and compare it directly to the output of PnP. Measurement vector is of size 7. Much easier filter to implement, complexity doesn't scale as a function of number of correspondences, but measurement covariance is hard to estimate.

I would certainly like to try (2) above.

[danieldyer]

I've been meaning to revisit this for a while — for your use case this functionality does seem appropriate.

One thing you've probably already considered is that the attitude quaternion error distribution from your external pose estimation algorithm may be more non-Gaussian than fusing the image-plane coordinates directly, so I expect that the two formulations would not quite be equivalent. I suppose that if N were large the extra computational cost may become prohibitive, though.

[LukeAI]

This would also useful for my use-case - I am trying to stabilise detections of objects in pointclouds - so I get a stream of x,y,z, quaternion coordinates and I apply a UKF to smooth the output. (which is also x,y,z, quat)

[LukeAI]

I've just set the state and measurement vector as UKF::Vector<4> - does that sounds reasonable? Or is there some special internal quaternion-aware handling that I would benefit from if I used the UKF::quaternion as the internal state?

[danieldyer]

I'd definitely use the Quaternion class to represent orientation in the state vector, since that's explicitly supported.

Using a plain 4-vector in the measurement vector should work to some extent, but definitely not as well as it could since the expected measurement vector calculated from the sigma point distribution may be de-normalised.

As long as you're using the Quaternion class in the state vector, the resulting a posteriori state estimate will have a correctly normalised quaternion component, but your errors probably won't be as low as they would be if the measurement vector class properly supported quaternion components with the appropriate averaging routine like the state vector does.

I'm finally getting around to updating some dependencies and getting the library to compile properly on the latest clang and gcc, so I may get around to this in the next few days.

[LukeAI]

Unfortunately I don't seem to be able to get it working with Quaternion in the state vector and Vector<4> in the measurement - could you advise how to convert a UKF::Quaternion into a UKF::Vector<4> ? I've tried a few things but they all give build errors.

[TobiSchluter]

@LukeAI UKF::Quaternion().coeffs().eval() ? The call to eval gives the resulting Vector<4> instead of an expression which may be what causes trouble

[danieldyer]

Just be aware that if you're converting back and forth between Quaternions and 4-vectors using Eigen, the order of the coefficients differs between the constructor and the internal representation (w, x, y, z for constructor but x, y, z, w internally). See https://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html#ad30f4da9a2c0c8dd95520ee8a6d14ef6.

Another way of converting between Quaternions and 4-vectors in Eigen that you might like to consider is the Map class: https://eigen.tuxfamily.org/dox/classEigen_1_1Map_3_01Quaternion_3_01__Scalar_01_4_00_01__Options_01_4.html

[bendyer]

Issue cleanup—this was resolved by #59