Initialize using gravity vectors to exclude the influence of the orientation during initialization

[Zhaosn]

The previous method of subtracting gravity when calibrating accelerometers required that the Z-axis is upward, but now a gravity vector is used.
Now, when I initialize the module with the Z-axis not facing up, the first few returns will be around 0 and then jump to the actual value, instead of the initialized position as ypr=0 (which is what I thought it would be before). And it doesn't have this problem: #77
It's worth to note that my tests show it's maybe better to keep the code unchanged and keep the z-axis calibrated upwards, just make sure the z-axis was calibrated upwards. Now all directions can be just as bad, and the calibration error is related to epsilon.
(fix #78)

[Zhaosn]

Unfortunately, the bugs in this code don't always recur, causing me to repeat changes, and I apologize for the frequent interruptions.
If the acceleration reading is [0 0 0] at the beginning, it causes the unit vector to return [nan nan nan], which causes the ITerm always be nan, leading to unconvergence
fix: line3439

mpu6050/src/MPU6050.cpp

Lines 3438 to 3443 in fbb3de4

	for (int i = 0; i < 3; i++) {
	if (isnan(Reading[i])) {Reading[i] = 0;}
	Error = -Reading[i];
	eSum += abs(Reading[i]);
	PTerm = kP * Error;
	ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001

[xpeqex]

Hi @Zhaosn

Thank you for the contribution. Could you please share more details about the bug you are mentioning? Additionally, is there any setup you can share with us to test this?