IMU calibration routines are incorrect

[cheater]

Hi! Thanks a lot for making the PSVRTracker. It's a great library and very useful to the VR community. I really appreciate everyone who worked on it.

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

In this file:

ServiceRequestHandler.cpp#L1473-L1479

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 CalibratedAccel = raw_accel - raw_accelerometer_bias (I don't include the X Y and 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 PSVRTracker, 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 PSVRTracker 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;
} PSVRMat33f;

/* 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. */
PSVR_PUBLIC_FUNCTION(PSVRMat33f) PSVR_Vector3fFindRotMat(const PSVRVector3f *from, const PSVRVector3f *to);

/* Rotates vector `a` by matrix `m`. This is just matrix-vector multiplication that results in a new vector. */
PSVR_PUBLIC_FUNCTION(PSVRVector3f) PSVR_Vector3fRot(const PSVRMat33f *m, const PSVRVector3f *a);

/* ServiceRequestHandler.cpp */

PSVRResult ServiceRequestHandler::set_hmd_accelerometer_calibration(...)
{
    ...
    const PSVRVector3f *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. */
    ...
    PSVRMat33f *rotCal = PSVR_Vector3fFindRotMat(&measured_g, &ideal_g);
    PSVRVector3f *rotated_g = PSVR_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 */
    ...
}

PSVRResult ServiceRequestHandler::set_hmd_gyroscope_calibration(
    const PSVRHmdID hmd_id,
    const PSVRVector3f &raw_gyro, /* note that I've renamed this from raw_gyro_bias to raw_gyro */
    const float raw_variance,
    const float raw_drift,
        const PSVRMat33f &rotCal) /* note that I've added this parameter, it comes from set_hdm_accelerometer_calibration.
        for this reason, set_hdm_accelerometer_calibration must run before set_hmd_gyroscope_calibration. */
{
    ...
    const PSVRVector3f *rotated_gyro = PSVR_Vector3fRot(&rotCal, &raw_gyro);
    config->rotated_gyro_bias = rotated_gyro; /* note that there is no raw_gyro_bias any more. */
    ...
}

/* MorpheusHMD.cpp */

void MorpheusHMDSensorFrame::parse_data_input(...)
{
    ...
    PSVRVector3f *raw_accel = { raw_accelX, raw_accelY, raw_accelZ };
    PSVRVector3f *raw_gyro = { raw_gyroPitch, raw_gyroYaw, raw_gyroRoll };
    ...
    RotatedAccel = PSVR_Vector3fRot(config->rotCal, raw_accel);
    CalibratedAccel = PSVR_Vector3fScale(&RotatedAccel, config->accelScale);
    ...
    RotatedGyro = PSVR_Vector3fRot(config->rotCal, raw_gyro);
    CalibratedGyro = PSVR_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 PSVRTracker. 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 PSVRTracker (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!

[m-7761]

I'll check over all of this, but I just want to add up front I'm pretty sure iVRy wouldn't be making use of this project in any capacity. I will definitely read over and investigate all of this you've brought to attention. If you do have knowledge of what Sony's PS4 drivers do please elaborate if you can.

[cheater]

I don't know what PS4 drivers are doing, but this is a logical first step towards a correct implementation. iVRy recently released a version of PSVRTracker integration.

[m-7761]

I happened to hear iVRy was thinking of doing something like that a little while back, but it seems very strange to me. The project owner was looking at Monado. The thing is this is like a research project, so unless the code here is being worked on by iVRy developers that they should have a fork of it somewhere if so. Hmm, so the iVRy program is using the "client" layer without invoking the SDL stuff? How is it impacting VR software's performance? Does it require a doubly beefy system to host a game and PSVRTracker together?

I wanted to do an overhaul of this library this year to support my game software. (Edited: https://swordofmoonlight.itch.io/k is a demo published 1wk later. )I've been really impressed by the PSVR and Moss game (that the owner was the VR peripheral person for) lately. But I'm glad to know iVRy is using it since that suggests it would be disruptive to iVRy if I overhaul the project. I will have to stake out a different place to host my work and probably not publish it as PSVRTracker now.

Edited: For the record I have worked on this project before its leader left. And I've used it with my software, without tracking, but I tend to use the PSVRToolbox because the SDL window can't be hidden and redraws itself in real-time even when minimized somehow... assuming it can be. But I use it over some UDP code I added since I felt like writing a client interface was premature. (I feel like I need to rewrite the code to be able to follow it, or maintain it, since it's very byzantine style code.)

[cheater]

He based the PSVRTracker integration on an unmodified build of the latest master.

Updating PSVRTracker shouldn't be a problem at all. Make it as good as you can. He can always build from the same git hash. It doesn't matter. It's not disruptive to iVRy. He only put in the PSVRTracker integration but isn't really interested in supporting it at all.

[m-7761]

Thanks for more info. I do work on code full time for months on end, when I take on a project. I guess I don't understand what iVRy's use case is. I assumed if using this code it was in order to solve a problem that couldn't be otherwise solved. I know they were trying to get camera tracking working is all.