Sensor Fusion

Introduction

In PSMoveService, there are two mutually exclusive ways we may use the term 'sensor fusion'. First, we may use sensor fusion to combine readings from different sensors to estimate orientation. Second, we may use sensor fusion to combine readings from even more sensors to estimate both orientation and position (i.e. the pose).

When we first started PSMoveService, we used an orientation filter to do sensor fusion to estimate the orientation only and the position was pulled directly from the optical tracker. Now we use a pose filter to do sensor fusion to estimate the orientation and position together.

In both cases, we use the set of sensors in the inertial measurement unit (IMU). In the PSMove controller, the IMU has 3 sensors:

• Accelerometer - Measures the direction of gravity
• Magnetometer - Measures the direction of your local magnetic field (basically a 3d compass)
• Gyroscope - Measures the rate at which the controller is rotating about the controller local X, Y and Z axes.

TO DO: IMAGE DEPICTING EACH SENSOR AND THE XYZ ORIENTATION. ALSO SENSOR OFFSETS

And we also use an optical tracker (Optical-Tracker-Algorithms) to get a first estimate of the position.

#Calculating Orientation (only)

PSMoveService originally calculated orientation in its own filter, independent of any

You could track a controller's orientation just by adding up the deltas from the Gyroscope but there are two problems with this:

1. You need to start from a known orientation
2. Errors in the gyro readings will accumulate and within a few seconds the orientation will be completely off

Another way to determine orientation is to use a sensor that measures some locally static field pointing in a known direction like Earth's gravity. For example, if you know that your controller is standing upright when your sensor says gravity is pointing down, the controller's local Y-Axis and you are currently measuring gravity pointing down the controller's local X-Axis - then the controller must be rotated 90 degrees. To measure the direction of gravity the PS Move uses a 3-Axis Accelerometer.

Measuring the direction of one field allows you to compute two rotation values, typically yaw and pitch. Measuring the direction of two different fields pointing in different directions allows you to compute all 3 rotation values: roll, pitch, and yaw.

Just as in the single field case, if you measure two independent fields in one known controller pose (i.e. calibrate the controller sitting upright) and then compare those to current readings you can compute orientation by determining the rotation you would need to align the calibration readings with the current readings. Using the PS Move's 3-Axis Magnetometer we get the second field direction we need from the Earth's magnetic field.

So then if we can completely compute the orientation from the accelerometer and the magnetometer, why do we care about the gyroscope? The answer is noise and bias. Both the accelerator and magnetometer give much "noisier" readings than the gyroscope does. If you compute orientation from just the accelerometer and the magnetometer you'll get a really jittery looking orientation.

If however we can compute some weighted blend of the orientation computed from the Accelerometer-Magnetometer derived orientation with the Gyroscope readings we can get the best of both worlds: smooth orientation changes without the drift. This process of combining information from multiple sensors is called "Sensor Fusion". There are many algorithms for doing Sensor Fusion. They vary in what kinds of sensors they use, how they deal with noise, whether they need calibration or not, and how computationally expensive they are to run.

The PSMoveService library uses a modified version of Madgwick's AHRS orientation filter which I adapted from his paper describing the filter. His original method and implementation require no calibration because the default magnetometer direction is supposed to converge on the correct value. In practice I've found that it never quite did this for me and the filter suffered from drift in many circumstances as a result. For increased reliability (at the cost of ease of use) we instead do a manual calibration of the magnetometer. This calibration process is outlined on the PSMove Magnetometer Calibration wiki page.