gyro drift correction function added in MARG implementation
fix initialization of initial rotation

Thanks to PaulKemppi! Closes #4.