Added interpolation between values of the EGM96 matrix to get the geoid separation

Tested via software, not on the real hardware. This time Geoid.cpp is modified in one commit only.