Gyro calibration experiments with a turntable

Last weekend, I've dug out an old turntable to see how well the gyroscope of the Move can be calibrated with the USB-based calibration blob. The turntable has the advantage that it has a known rotation speed (two modes: 33 RPM and 45 RPM), so this can be used to see if the values we get back from one of the gyro axes somehow relates to real-world values.

Before I tried the turntable method, I just played around with the raw Gyro values to see what I can get out of them. I wrote a very simple QGraphicsView-based GUI to see the output visually, and this is what came out of that example:



As you see, that was not really anything to write home about, so next up was the turntable experiment. With that, I could scale the raw gyro readings so that "1.0" (in my case) corresponds to e.g. 45 RPM. Coupling that with an audio player using Qt MultimediaKit, one can translate the turntable movements into playback rate values and control the media player just as if it were a vinyl record:



In this week, I've been working on perfecting the calibration algorithm, cleaning up the API for the calibration part of the library and hooking everything up to Sebastian Madgwick's AHRS algorithm and visualizing the result with Qt3D.

New labs application: Sensorfilter

If you've been watching the PS Move API repository recently, you might have noticed the new "labs/" subdirectory. In there, I'll push some small utilities that I use for debugging and visualization of the current inner workings of the library. The first tool to be put there is "sensorfilter", which is a quick visualization utility that I wrote for testing the new sensor filtering and calibration APIs. It makes use of both PSMoveFilter and PSMoveCalibration, as well as the original PSMove API. With a properly calibrated controller, you can get good readings (again, I've moved the controller a lot for this screenshot):

The slider at the left controls the current low-pass filter implementation's alpha value (i.e. how quickly should the sensor values converge to the newly-read value). As the sensor filter API is kept modular, it's possible to try to stick other sensor implementations in there without having to change client applications (of course, if there are tweakable settings, the client application has to know about these). With the Sensor Filter utility, it's easy to try out new filters and to sanity-check the calibration code.

The utility is available on github.com/thp/psmoveapi in the "labs/sensorfilter" subdirectory. Have a look at the README file to find out how you can build it. It depends on Qt 4 (tested with 4.8).

Plans for the next few days:

• Have a look at the OpenCV status, provide feedback to Benjamin
• Try improved sensor filtering algorithms and compare them
• Finish the calibration backend code, supporting USB "calibration blob" modes
• Clean up and document the code, extend the Python and Java bindings

Sensor calibration: Custom method and calibration blob

In the last few days, I've been working on getting a basic sensor data filtering infrastructure set up. In addition to that, I've added support for getting and storing the calibration data that is saved on the controller (the axis naming is a bit different in the PS Move API compared to what you will find on that Wiki page). In addition to the factory-set calibration data, I've also implemented support for a "custom" calibration scheme where the user has to do a 6-point tumble test, which will be used as anchor points for calibration.

The custom calibration scheme works a bit like "mccalibrate" from linmctool, but has (at the moment) a bit simpler algorithm (taking the average over 200 sensor readings). The new calibration tool that I wrote (c/calibrate.c) can detect if you have moved your controller too much while the readings were taken, and will ask you to do the given position again. A custom calibration could look like this (I've moved the controller a lot for the first "buttons up" reading to demo the move detector code):

~S/psmove/psmoveapi% build/calibrate 
Serial number: 04:76:6e:XX:XX:XX
Put the controller in the position 'bulb up' and press the Move button
All readings done for bulb up.
bulb up:
a (avg:     1 |  4359 |   188)
a (dev:    20 |    13 |    43)
m (avg:     2 |    -8 |  -421)
m (dev:     4 |     8 |     5)

Put the controller in the position 'bulb down' and press the Move button
All readings done for bulb down.
bulb down:
a (avg:  -165 | -4379 |  -113)
a (dev:    30 |    20 |    48)
m (avg:   -69 |   287 |  -435)
m (dev:     5 |    10 |     5)

Put the controller in the position 'buttons up' and press the Move button
All readings done for buttons up.
buttons up:
a (avg:   177 |    62 |  4173)
a (dev:  3940 |  2079 |   987)
m (avg:   -34 |    57 |  -250)
m (dev:    22 |    16 |     6)



  DEVIATION TOO HIGH - PLEASE RETRY

Put the controller in the position 'buttons up' and press the Move button
All readings done for buttons up.
buttons up:
a (avg:   -41 |   358 |  4362)
a (dev:    22 |    19 |    19)
m (avg:   -29 |    77 |  -250)
m (dev:     5 |    10 |     5)

Put the controller in the position 'buttons down' and press the Move button
All readings done for buttons down.
buttons down:
a (avg:  -128 |   422 | -4343)
a (dev:    28 |    21 |    25)
m (avg:   -61 |    84 |  -515)
m (dev:     5 |    10 |     7)

Put the controller in the position 'buttons left' and press the Move button
All readings done for buttons left.
buttons left:
a (avg:  4252 |   188 |    63)
a (dev:    38 |    41 |    49)
m (avg:    96 |    76 |  -392)
m (dev:     4 |    13 |     8)

Put the controller in the position 'buttons right' and press the Move button
All readings done for buttons right.
buttons right:
a (avg: -4458 |   338 |   -82)
a (dev:    26 |    24 |    35)
m (avg:  -187 |    85 |  -369)
m (dev:     5 |    13 |     6)

Now that we have done a calibration run, we need a tool to display the results (also, we need a tool that reads the data from USB and stores it): Enter "dump_calibration". This tool will read and persist all calibration blobs of connected USB controllers (the "calibrate" tool will only store custom calibration, and only for Bluetooth controllers). When run with a Bluetooth controller (and again assuming that you have already done the USB fetching part), you can get output like this:

~S/psmove/psmoveapi% build/dump_calibration 
File: /Users/thp/.psmoveapi/04_76_6e_XX_XX_XX.calibration.txt
Flags: 3
Have USB calibration:
10 00 67 07 4f 7f a4 7f c2 90 68 6e 25 80 05 80
60 7f 10 80 bf 6e 75 90 c6 7f c5 7f c1 7f bb 90
33 80 47 7f c7 6e 90 7f d2 08 db 7f 57 80 47 80
d7 07 d2 7f 58 80 4b 80 00 00 00 00 00 00 00 00
00 01 ce 08 e0 01 04 97 53 80 5b 80 e0 01 cc 7f
7b 90 39 80 e0 01 dd 7f 4d 80 64 94 f4 07 d1 d7
12 41 72 fc d0 c0 c9 3e 0d c2 a4 1c 6f 3f a9 90
7b 3f 37 5c 71 3f 02 1d 32 3f 87 69 a1 3d 00 00
00 00 00 00 00 00 00 00 00 00 00 00 00 00 00
# Orientation #0: ( -177 |   -92 |  4290)
# Orientation #1: (-4504 |    37 |     5)
# Orientation #2: ( -160 |    16 | -4417)
# Orientation #3: ( 4213 |   -58 |   -59)
# Orientation #4: (  -63 |  4283 |    51)
# Orientation #5: ( -185 | -4409 |  -112)
# Gyro X, 80 rpm: ( 5892 |    83 |    91)
# Gyro Y, 80 rpm: (  -52 |  4219 |    57)
# Gyro Z, 80 rpm: (  -35 |    77 |  5220)
# byte at 0x3F: 00

Have custom calibration:
         ax         ay         az         mx         my         mz
#0:       1.27    4359.10     187.74       2.04      -8.18    -421.33 
#1:    -164.57   -4378.57    -112.98     -69.21     286.53    -435.20 
#2:     -41.38     358.21    4361.73     -28.52      77.03    -249.96 
#3:    -127.78     421.94   -4342.93     -61.33      83.87    -514.74 
#4:    4251.81     187.99      62.83      95.57      75.67    -391.71 
#5:   -4458.25     338.40     -81.67    -187.27      85.35    -369.02 

This calibration file can be used by the new PSMoveCalibration API that wraps a PSMove object and provides calibration features on top of it. The function that users will probably use most is psmove_calibration_map() - it takes as input 3, 6 or 9 integer values and converts them into corresponding float values that have been normalized.

With the tumble test ("custom calibration"), we only get values for the accelerometer and magnetometer - for calibrating the gyro, we would need to have access to a turntable and control its speed - something that's not impossible to do, but very hard. Thanks to the research done by other MoveOnPC people, we can extract the information from the USB calibration blob - it stores the expected readings for 80 rotations/minute (according to the wiki page).

You can find the new code on github.com/thp/psmoveapi - expect some rough edges and more updates in the coming days and weeks :)

Profiling sensor reading performance

Right now, the PS Move API is single-threaded. This makes debugging easier, and simplifies the code and usage of it. What this also means is that setting the LED colors will slow down sensor reading (not taking into account any Bluetooth communication slowdown that might happen even in multi-threaded scenarios).

In order to get high-quality sensor readings, the read rate should be as high as possible. On my MacBook Pro in OS X 10.7.4, I currently get an average of 83.49 sensor readings per second if I don't send any LED updates. If I continuously send LED updates (one update after every sensor reading), the reading rate drops to 51.02 sensor readings per second (thats ~ 61% of the best performance).

In order to improve the situation while still being able to set the LEDs (after all, we need the LEDs on for tracking the controller position with the camera) there are two options: First, if the color is static (i.e. it does not change or does not change often), we only have to send an update every 5 seconds (the LEDs keep glowing in the set color for 5 seconds after an update). And even if we want to modify the color during tracking, we can be smart about the updates and ignore some requests if the update rate is too high.

I have implemented both variants in the latest PS Move API code (you can find it in the Git repository) now, and also wrote a small test application (which you can also find in the Git repository as "test_read_performance" when you build the source) to compare the different methods. Here are the results (Git revision 64a3214 on a 2.53 GHz Intel Core 2 Duo, static read = 4000ms between updates, smart read = 120ms between updates):


 -- PS Move API Sensor Reading Performance Test --


Testing STATIC READ performance (non-changing LED setting)
1000 reads in 12373 ms = 80.821143 reads/sec (56x seq jump = 5.60 %)
1000 reads in 11954 ms = 83.654007 reads/sec (29x seq jump = 2.90 %)
1000 reads in 12090 ms = 82.712986 reads/sec (36x seq jump = 3.60 %)
=====
Mean over 3 rounds: 82.396047 reads/sec


Testing SMART READ performance (rate-limited LED setting)
1000 reads in 16386 ms = 61.027707 reads/sec (283x seq jump = 28.30 %)
1000 reads in 16673 ms = 59.977209 reads/sec (304x seq jump = 30.40 %)
1000 reads in 16736 ms = 59.751434 reads/sec (306x seq jump = 30.60 %)
=====
Mean over 3 rounds: 60.252116 reads/sec


Testing BAD READ performance (continous LED setting)
1000 reads in 19593 ms = 51.038636 reads/sec (479x seq jump = 47.90 %)
1000 reads in 19732 ms = 50.679100 reads/sec (486x seq jump = 48.60 %)
1000 reads in 19471 ms = 51.358430 reads/sec (469x seq jump = 46.90 %)
=====
Mean over 3 rounds: 51.025391 reads/sec


Testing RAW READ performance (no LED setting)
1000 reads in 12105 ms = 82.610492 reads/sec (40x seq jump = 4.00 %)
1000 reads in 11987 ms = 83.423709 reads/sec (31x seq jump = 3.10 %)
1000 reads in 11843 ms = 84.438065 reads/sec (21x seq jump = 2.10 %)
=====
Mean over 3 rounds: 83.490753 reads/sec

The ideal situation is RAW READ (not updating the LEDs at all). STATIC READ is when we set the LEDs to a fixed color and only send an update every 4 seconds - the performance impact is not really noticeable in practice. When we try to update the LED colors all the time, but have the new rate-limiting feature enabled (SMART READ), performance drops to 60.25 reads per second (that's 72% of the best performance), but we get an LED update every 120 ms (that's the current threshold for rate limiting and might change in the future). And as discussed above, if we don't do any rate-limiting (BAD READ), the performance is even worse.

The "seq jump" counts are the non-continuous readings of the sequence number that the controller sends with every report. A "seq jump" means that we missed one (or more) sensor readings from the controller. As the sequence number is something internal to the controller, I'm not sure if we can get any better than the 2-4% "seq jumps" in the RAW READ case, but it's good to know that we can nearly read as fast as the controller itself is able to process the data internally.

In practice, even if we do update the LEDs while tracking, we might not update the color every 120 ms and therefore the practical read performance should be between 60.25 and 83.49 reads per second.