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 be exactly parallel to and upward of the direction of gravity, but now a normalized 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) (I didn't test what the value of yaw would be at this point). And it doesn't have this problem: #77

This algorithm for obtaining the normalized vector could perhaps be replaced by "Fast Inverse Square Root".
Variable naming may not standard

[Zhaosn]

Oh no, the normalization of the gravity vector should stop when the Magnitude is less than 16384, otherwise it will cause oscillation rvt/headtracker#10 (which didn't happen in the example but in other project). I have identified the problem now and am preparing to upload improved code.