IMU calibration routines are incorrect

[cheater]

Hi! Thanks a lot for making the PSMoveService. It's a great library and very useful to the VR community. I really appreciate everyone who worked on it. I've posted a similar issue to the PSVRTracker github repository, as the relevant code is essentially the same.

I've been trying to track down an issue that I've started experiencing in a software that uses PSMoveService called iVRy, and I've figured out where the issue lies within PSMoveService.

In this file:

ServerRequestHandler.cpp#L2887-L2893

you compare the measured gravity vector to a straight down vector, create a difference vector (raw_accelerometer_bias), and then you calibrate everything by adding that difference. So in MorpheusHMD.cpp you do CalibratedAccel = raw_accel - raw_accelerometer_bias (I don't include the .i .j .k and X Y Z). You do it the same way for both accel and gyro.

That is however not correct. I'll explain below what that affects, but first let me present how the calibration could be done in a better way. You would find the rotation between the measured gravity vector, and the ideal straight-down gravity vector, and then you would find a linear matrix (call it rotCal) that rotates the measured gravity into the straight-down vector. Only after that rotation is performed, should you perform any scaling - not before. After you do the rotation, take measured_g, rotate it by the matrix, and figure out its length - the inverse of that number is your scaling calibration number for the accelerometer, call it accelScale. Next up, to calibrate the gyro, take the raw gyro data, and rotate it with rotCal. After this, get the numbers from the gyro, and those are your bias numbers (call that rotated_gyro_bias).

The difference might not be big for most headsets, however I have a headset where the IMU's internal MEMS gyroscope seems to have drifted out to a pretty steep angle. It has a clear measurement error when using PSMoveService, however it works perfectly when used with the PS4 using Sony's algorithms. I've found out that those IMU chips actually don't have any guarantee of how they will drift and it's normal for them to drift out of calibration over time. At first the author of iVRy said my IMU might be broken or maybe it moved physically, but I took apart my PSVR v2 and the IMU is located in such a way that the PCB (printed circuit board) can't move at all, not one bit, and it's not physically broken. So I figured that the IMU might have drifted internally and sure enough, that is a common mode of operation for those chips. It's not a mode of failure as the chip didn't fail - it is expected to do that. Essentially, there are tiny gyroscopes inside the chip, and the angle at which those rotate can change over time. This is what happens to all these chips, and in my case it's stronger than in other headsets.

So this is what the headset looks like with just basic calibration. You can see that the view is permanently tilted to the side. And if you try to calibrate it using the code that exists in PSMoveService as of today, this is what it looks like. As you can see, if you look straight ahead it looks good, however, if you turn your head, then two things are apparent. First of all, the raw_accelerometer_bias is applied incorrectly when the head is turned 90 degrees to either side, making the tilt worse. Second of all, there's this weird drift that happens right after the headset is moved. This is because of how raw_gyro_bias is calculated currently.

So the fix is to instead rotate the data coming from the IMU, because physically, that's what happens - it's almost as if the PCB inside the headset is at an angle. Here's a good a good link to how you can quickly calculate the rotation matrix between two vectors (Math Stackexchange).

So the pseudocode for the new calibration would be roughly like you can see below. Bear in mind I'm not a C++ programmer, so the &s and *s might be wrong.

/* ClientGeometry_CAPI.h */

/* 3x3 matrix of floats with this shape:
a b c
d e f
g h i
*/
typedef struct
{
    float a, b, c, d, e, f, g, h, i;
} PSMMat33f;

/* Finds a matrix that rotates `from` to `to`, without changing its
length - the length of `from` after rotation might be different
than the length of `to`, but they will point in the same direction.
See Math Stackexchange link above for details on how to
implement this function. */
PSM_PUBLIC_FUNCTION(PSMMat33f) PSM_Vector3fFindRotMat(const PSMVector3f *from, const PSMVector3f *to);

/* Rotates vector `a` by matrix `m`. This is just matrix-vector multiplication that results in a new vector. */
PSM_PUBLIC_FUNCTION(PSMVector3f) PSM_Vector3fRot(const PSMMat33f *m, const PSMVector3f *a);

/* ServerRequestHandler.cpp */

void handle_request__set_hmd_accelerometer_calibration(...)
{
    ...
    const PSMVector3f *ideal_g = {0, 0, 1}; /* the ideal downwards gravity vector. I don't know which axis is downwards, so I assumed it is z=1. */
    ...
    PSMMat33f *rotCal = PSM_Vector3fFindRotMat(&measured_g, &ideal_g);
    PSMVector3f *rotated_g = PSM_Vector3fRot(&rotCal, &measured_g);
    float accelScale = 1/sqrtf(rotated_g.x*rotated_g.x + rotated_g.y*rotated_g.y + rotated_g.z*rotated_g.z);

    /* you store rotCal and accelScale in the calibration data */
    ...
}

void handle_request__set_hmd_gyroscope_calibration() 
{
    ...
    /* note that I've renamed raw_bias to raw_gyro.

    I've also added rotCal, it comes from handle_request__set_hmd_accelerometer_calibration.
    for this reason handle_request__set_hmd_accelerometer_calibration must run before
    handle_request__set_hmd_gyroscope_calibration. */

    const PSMVector3f *rotated_gyro = PSM_Vector3fRot(&rotCal, &raw_gyro);
    set_config_vector(rotated_gyro, config->rotated_gyro_bias); /* note that there is no raw_gyro_bias any more. */
    ...
}

/* MorpheusHMD.cpp */

void MorpheusHMDSensorFrame::parse_data_input(...)
{
    ...
    PSMVector3f *raw_accel = { raw_accelX, raw_accelY, raw_accelZ };
    PSMVector3f *raw_gyro = { raw_gyroYaw, raw_gyroPitch, raw_gyroRoll };
    ...
    RotatedAccel = PSM_Vector3fRot(config->rotCal, raw_accel);
    CalibratedAccel = PSM_Vector3fScale(&RotatedAccel, config->accelScale);
    ...
    RotatedGyro = PSM_Vector3fRot(config->rotCal, raw_gyro);
    CalibratedGyro = PSM_Vector3fAdd(&RotatedGyro, &config->rotated_gyro_bias);
    ...
}

Note one thing. When calibrating the accelerometer, you can only ever get the scaling in one direction. To fully calibrate in x, y, and z, you would have to have a 3D jig that can rotate the headset so the gravity is first downward (normal calibration), then so gravity is towards the side of the headset, and then once more so that gravity is towards the front of the headset. It's crucial that you rotate the headset by a somewhat precise angle of 90 degrees. You could do that by making a box to put the headset in. The box has 90 degree angles, so you will be able to rotate it on its sides and it will stay in the right position. However, for this to be correct, the headset would need to sit tightly inside the box. For this reason you could create small cardboard cutouts that you stick in the box, and the headset rests on them.

Also note that you could theoretically calibrate the gyro scaling, but to do that you would need to put the headset-in-a-box (like described above) on a motorized turntable with a revolution sensor to measure the angular velocity of the turntable. A revolution sensor tells you when a full revolution of the turntable has happened. It can be done cheaply. In that case, during calibration, you would first calculate rotated_gyro as above, then config->rotated_gyro_bias as above, and then after that you would calibrate the scaling for each of the 3 rotation axes, in separate runs.

One more thing to note. I assume that the accelerometer measures the gravity vector with no bias (ie constant component), and I assume there's just a scaling error. This is somewhat of a large assumption. To properly test it, you would need to put the IMU in vertical free fall to see if it registers any sort of bias, and then you could figure out the bias and scaling separately. You would have to perform the test in all three directions. This is however not very practical. And without that, you can only assume either a bias or a scaling error, and I assume a scaling error. With a bit of work towards automatically cleaning up the sensor data, you essentially could drop the headset inside like a soft foam carrying case, and nothing bad would happen to it, mechanically speaking. As in it wouldn't break. But it would be a bit more involved than the code above, you would have to cull the recorded data to just the period where it's clearly in free fall (which can be detected), and you would have to somehow correct for the pull of the cable and make sure that it's not rotating at the same time - or use the rotation as an additional source of data, and calculate the calibration that way. It's an interesting problem in classical mechanics, that's for sure, but it's by no means very difficult. Another thing you could do is put the headset on the perimeter of a round turntable - in that case it would always be experiencing some sort of acceleration, which you could calculate on paper, and compare to what the headset's IMU has measured. The turntable would not only need a revolution sensor (i.e. a sensor that tells you when the turntable has turned a full 360 degrees), but also a position sensor that measures the angle at which the turntable is with precision comparable to the 2000Hz sampling rate the headset's IMU works at.

Another thing is the fact that even after all this calibration, a short term calibration will not find the long term drift that happens when using PSMoveService. Invariably it's a drift to the left, like you're slowly turning your head to the right, and it is only noticeable after 10 minutes or so, but it is invariably there and in all headsets. I think it should be calibrated separately from the calibrations described above. I think the source of this drift might be either integration error in PSMoveService (i.e. when you add floats many many times over, then there's a statistical error that becomes more significant over an hour or so), or it could be an intrinsic data bias in the output of the IMU being used in the PSVR. I think it should be calibrated separately because properly calibrating it away takes at least 10 minutes of measurement, and so you should be able to perform the quick calibrations described above in the pseudocode without erasing the long-term-drift calibration data, which takes ages to do, so people won't want to do this every time they turn on the headset. However, that is a conversation for another time. Thanks for reading this very long post! And thanks for your work!

[cboulay]

Hi @cheater ,

This all seems like great information.

Unfortunately, I haven't used my PSMove controllers in years and I don't see when I'll have time to test this out. If you can put together a pull request then I can make a test build and upload it and ask users to try it out. If it seems to work then I can add it to the release page.

[cheater]

Hi @cboulay, thanks. This is specifically about the PSVR headset - why are PSMove controllers relevant? Thanks

[cboulay]

Some of the code is common across devices. Any fix for PSVR would have to not-break PSMove controllers. I'm optimistic that it could also improve PSMove controller operation.

[cboulay]

Sorry, I didn't fully understand the scope of the proposed changes. I don't have a PSVR and I've never looked at the HMD-exclusive code. I thought this was a more fundamental change to the way the IMU was calibrated. But it seems like it's scoped to only the HMD. In that case, I would have no way to test it.

[cheater]

Some of the code is common across devices. Any fix for PSVR would have to not-break PSMove controllers. I'm optimistic that it could also improve PSMove controller operation.

Sorry, I didn't fully understand the scope of the proposed changes. I don't have a PSVR and I've never looked at the HMD-exclusive code. I thought this was a more fundamental change to the way the IMU was calibrated. But it seems like it's scoped to only the HMD. In that case, I would have no way to test it.

If this code is representative of IMU calibration in other places in this code (including PSMove and other devices, even non-Sony) then it likely all needs to be updated in this way. However, it's lucky that the changes aren't /huge/ per se.

[cheater]

Can you suggest an open source project that uses PSMoveService to provide SteamVR tracking for the PSVR? If I can get that to build, then maybe I could try to make this into a PR.

[cheater]

I've also had another question. For both the accelerometer and gyroscope, you store the data variance during calibration. What is that used for?

[cheater]

I'm asking about the variance because depending on what it is used for, it may have to be re-calculated after calibration is complete.

[cboulay]

Can you suggest an open source project that uses PSMoveService to provide SteamVR tracking for the PSVR?

Maybe https://github.com/HipsterSloth/PSMoveSteamVRBridge ? I've never used a PSMove via SteamVR so I don't know how this works.

I haven't heard from him in a while, but maybe @HipsterSloth knows.