A C++ implementation of a nonlinear 3D IMU fusion algorithm