inertial navigation class in C++