fusion

[liuping001]

Where I can find magnetometer,gyroscope,accelerometer fusion algorithm of psmovecontroller

[cboulay]

There are at least 2 different algorithms implemented. Here is 1 of them:

https://github.com/cboulay/PSMoveService/blob/master/src/psmoveservice/Filter/KalmanPoseFilter.cpp

This isn't really an "issue". Please post general questions to the google group: https://groups.google.com/forum/#!forum/psmoveservice

[HipsterSloth]

As Chad mentioned there are a few different position and orientation filter algorithms. The orientation filters handle the magnetometer, gyroscope, and accelerometer sensor fusion.

This file has the Kalman Orientation filter for the PSMove and DualShock 4 controllers: https://github.com/cboulay/PSMoveService/blob/master/src/psmoveservice/Filter/KalmanOrientationFilter.cpp

This file has 4 different orientation filters: https://github.com/cboulay/PSMoveService/blob/master/src/psmoveservice/Filter/OrientationFilter.cpp

• OrientationFilterMadgwickARG
  • Implementation of the Magwick's AngularRate-Gravity filter (Gyro+Accel)
• OrientationFilterMadgwickMARG
  • Implementation of the Magwick's Magnetic-AngularRate-Gravity filter (Mag +Gyro+Accel)
• OrientationFilterComplimentaryOpticalARG
  • A complimentary filter that blend optically derived yaw with the MadgwickARG filter
• OrientationFilterComplimentaryMARG
  • A complimentary filter I derived from Madgwick's MARG filter
  • Optimizes a best fit quaternion that aligns magnetometer and accelerometer readings recorded at calibration time with the currrent magnetometer and accelerometer readings
  • Blends with current gyro integration
  • This is the default orientation used for the psmove controller at the moment

Finally there is the KalmanPose filter Chad linked to. This is based off of a full pose (orientation+position) kalman filter Chad originally wrote in MatLab. I tried to port it to C++ but never got it working correctly. I wouldn't use that for anything at the moment (except maybe for reference).

Let me know if you have any questions about any of these filters

EDIT: Here is Madgwick's paper that is the basis for all of the Madgwick orientation filters: https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf

[HipsterSloth]

Also if you are looking for a more drop in implementation of a Madgwick orientation filter there is a standalone implementation here of the MARG and ARG filters: http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

A few things to note about that implementation:

• They made some simplifications in the math to assume that +Z is up (i.e. the direction opposite gravity)
• That implementation is under the GPL license. I re-implemented the algorithm from info derived in the paper so that my code could be under a more permissive license (Apache-2.0) in case someone didn't want their project under the GPL.