How to find waypoints, with data from a gyro and accelrometer

So I have the task of making waypoints and waypoint trajectory with the information given to me by a gyro and accelometer

Using the output of the gyro I have the orientation in both Euler and Quat

Use the acclerometer, I take the current reading - the previous reading to get the instant acceleration. And using SUVAT (PreVAcc t+PrevVelo=Velo) I have the velocity. Using further SUVAT I can find a distance (Assuming the object begins at 0,0,0, I can then find the positional data) (s=Velo*t+0.5*a*t^2). I know when using the waypoint trajectory function it normally outputs this kind of data but I need it to work backwards and give me the way points.

Any pointers