Incorporating new Orientation Complementary MARG filter

[BinaryShift]

Derived from Madgwick AHRS paper

• Refactored Madgwick IMU and MARG filter
• Removed old Magdwick and Mahony external dependencies
• Added new filter derived from Madgwick filter that appears to eliminate drift in most cases.
• Updated magnetometer_calibration tool to include a calibration phase that records the magnetometer field when the controller is in the "identity" upright pose
• Added mechanism for transforming sensor readings to different coordinate frames (default is open_gl frame)
• Added new math sub folder with utilities used by the orientation system
  • psmove_alignment.cpp/.h - utility methods for aligning pairs of vector (used by orientation filters)
  • psmove_glm_math.hpp - wraps up glm headers + glm utility macros
  • psmove_math.c/.h - C math constants and utility macros used by orientation filter
  • psmove_quaternion.cpp/.h - wrappers for a few missing GLM quaternion functions used by orientation filter
  • psmove_vector.h/.c - methods for the PSMove_3AxisVector structure (used mostly for magnetometer vector transformation)

[BinaryShift]

Some additional notes:
This new orientation filter is basically just performing an iterative search using the same gradient descent algorithm that Madgwick outlines in his paper. The common ideas between my approach and Madgwick's are:

• For some arbitrary controller orientation you define an "identity pose" for controller.
  • Specifically, the accelerometer (gravity) and magnetometer readings from the controller in the "identity pose" represent the controller as having the identity orientation quaternion (1, 0, 0, 0)
• For any other accelerometer/magnetometer reading you determine what rotation quaternion you have to use to align the current sensor readings with the "identity readings"
• The rotation quaternion that solves this "alignment optimization problem" is the orientation of your controller
• You can solve for this quaternion using a gradient descent on an error function (how well your alignment quaternion aligns the current sensor readings with the identity sensor readings).
• Since this mag-grav orientation will be a bit noisy you can blend it with the integrated angular position from the gyros (integrated gyro position is smooth but has drift, m-g orientation can correct for drift).

In Madgwick's paper he does one gradient step once per frame. He also defines a way for the true identity sensor frame to be converged to over time (starting from a guess). This has the advantage that it requires no calibration of the identity frame and that it's fast.

However in practice this approach still appears to have a few problems:

• Some degree of drift is present if the gamma and beta parameters (blending weights) aren't tuned right
• It's not clear how to tune these weights for all situations
• The algorithm isn't very debug-able
• There is a "warm-up time" to getting to converge on the identity sensor frame

In my variation, I do the following:

• Explicitly calibrate the identity sensor readings since we already have a calibration tool.
• Don't just take one gradient step:
  • Do an iterative descent until we converge within an error tolerance
  • This is more expensive, but not really that bad on a PC (just some simple matrix math in a loop with 10-20 iterations).
• Start off heavily favoring the m-g orientation in the first few updates and then blend over to emphasizing the gyro integration (let the m-g orientation blending take care of the drift).

The cool thing about this approach is that it's a lot more explicit and inspect-able. You can adjust the m-g blend weight value to see that contribution or adjust it the other way and see pure gyro integration. You can look at the result of the m-g alignment (see psmove_alignment_quaternion_between_vector_frames) and see how good of an alignment was computed. You can step through alignment function and watch the error function converge. And the identity orientation is clear (computed at calibration time). The new code also uses glm math primitives so it's more clear what mathematical objects you are dealing with.

[rovarma]

@BinaryShift Sorry for the delay. I finally got around to trying this out.

Unfortunately, I'm getting pretty bad results. I'm not seeing drift, but the orientation I'm getting seems to be pretty far off from the real-world orientation. With the old algorithm, I was getting some yaw drift, but the orientation matched up well with real-world orientation.

Since a video is better than a thousand words (or something like that), I've made a short video to demonstrate.

In the video, I:

• Started test_opengl
• Moved/rotated the controller in front of the camera somewhat. You can already notice the discrepancy between real-world and "in-game" orientation
• At around 0:12 into the video, I reset the orientation using the PSMove button (you can see it snap to the "correct" orientation)
• The discrepancy does not go away after this reset.

For reference, I've attached the magnetometer calibration output (had to change the extension to .txt because GitHub doesn't accept .csv).

Let me know if you need any more information or want me to test something.

00_07_04_29_2f_ed.magnetometer.txt

[BinaryShift]

Thanks for giving this a test @rovarma! And also thanks for the video, that helps a lot. I think I know what you are seeing. One of the drawbacks with the new calibration approach is that the controller has to be aligned with the facing or your pseye camera like so:

Pardon my crude mspaint diagram. This is because you are basically defining what the global reference frame of the controller is in the second stage of the calibration. I could call that out better in the comment displayed on screen in magnetometer_calibration.

I can get the same effect you are seeing if I don't line up the controller facing. Here is a video showing the effect of having correct and incorrect alignment:

https://youtu.be/6mrzZXtroVo

Also I can't seem to get functional tracking on my psmove unless I have the exposure set to low, thus the nearly black background in my locally built version of test_opengl that you see in the video.

[rovarma]

Ah ok, thanks. I wasn't aware of that limitation. I'll give the "correct" calibration a try tomorrow.

I don't quite understand where that limitation comes from though. From what I gathered from the comments in the code, I assumed that when running calibration, you're basically determining what the "identity" pose is for the controller (ie. its orientation in real-world-space).

I had hoped that that would be the case, since orientation-in-real-world-space is exactly what I need for my use case, but from what you're describing, that's not actually what I'm getting? Unless I'm misunderstanding something.

If this is not how it works, is there a way to get the calibration/orientation to work without the requirement for alignment with the camera? Alignment with the camera is impractical for my use case, since there are multiple cameras.

EDIT: That's not to say that the requirement for alignment with the camera isn't good enough for regular PSMoveAPI use though. Just being selfish and trying to figure out how this all fits into what I need from it :)

[BinaryShift]

TL;DR This will work for your multi-camera case (but you might want to call some special setup functions)

So all that the second step of the magnetometer calibration is doing it recording the direction of the magnetometer vector when the controller is standing vertically (gravity is pointing downward). When the orientation filter gets a magnetometer/accelerometer reading from this recorded real world controller pose, the filter will return the identity quaternion.

The reason you have to line up the controller facing with the camera is really only to make the render model line up in the test open gl app. It defines the identity quaternion corresponds to the controller 3d model lying flat pointing at the screen/camera.

If you want to define some other coordinate system whose X/Y/Z axes are not aligned with any particular cameras frame of view you are free to do that. Simply align the controller facing along your coordinate system when running the second magnetometer calibration step. Then when running your application you'll get the identity quaternion when in this pose.

There are two new functions that might be useful to your use case:

• psmove_set_calibration_transform(PSMove *move, const PSMove_3AxisTransform *transform)
• psmove_set_sensor_data_transform(PSMove *move, const PSMove_3AxisTransform *transform)

Remember how I said that the filter returns the identity quaternion when the controller is standing upright facing the camera? Well the open_gl test app assumes that identity quaternion means that the controller is pointing at the screen (rotated about the X-axis 90 degrees). To fix this, I provide a "calibration transform" that rotates the calibration readings by 90 degrees. It's as if you did the second step of the calibration with the controller lying flat pointing at the screen. Since this is a hard pose to maintain in real life I have you record this calibration vertically.

However this is all done just to maintain reverse compatibility with how psmoveapi worked. Your application doesn't need to rotate the calibration data so you can just do this at startup:

psmove_set_calibration_transform(move, k_psmove_identity_pose_upright); // This is just the identity xform

The other wrinkle has to do with the coordinate system of the orientation filter. Before this pull request, the sensor data in the orientation code has been rotated 90 degrees clockwise about the x-axis. The original Madgwick orientation filter was coded to assume an OpenGL style coordinate system (+x=right, +y=up, +z=out of screen), rather than than PSMove's coordinate system where:

• +x = From Select to Start button
• +y = From Trigger to Move button
• +z = From glowing orb to USB connector

The current default sets the sensor transform to assume an OpenGL style coordinate system in order to maintain reverse compatibility, but you don't have to do this in your application. You can just leave the sensor data in it's natural coordinate system like so:

psmove_set_sensor_data_transform(move,k_psmove_sensor_transform_identity);

EDIT: Thanks for bringing this up BTW. This is an important subtle detail that I need to document more clearly and this discussion will help me make sure I have all of the bases covered. And I think your use case should be supported. I wan't to support multiple camera in my project as well (but we should talk about that in another thread).

[rovarma]

I see. Thanks for the detailed explanation; that makes perfect sense now.

I agree that this needs to be documented somewhere (readme, headers?), as the effects of "doing it wrong" are rather opaque if you're not intimately familar with how the filter works internally.

IMO this should be done before this PR is accepted (or as part of this PR), since this filter is now the default, which means people's existing projects will break if they redo the calibration wrongly, like I did.

[thp]

Can those be put into psmove_private.h? If not, maybe we should have API methods for getting/setting those values instead of extern declarations.

[HipsterSloth]

I just added the recommended changes to the sensor and calibration transform API. As for the magnetometer documentation I almost wonder if this would be best suited as a wiki page? I feel like pictures/video links are best for clarity. I have a previous wiki write up for this I could adapt if you like.

[thp]

Merged, thanks!