Straight line ramp path following altitude error

[afonsoVale]

When following a straight line ramp, the aircraft altitude doesn't match the desired altitude. This error increases with the distance from the path origin. When climbing the aircraft is higher than it should be and when descending, the aircraft is lower than it should be. The issue can be replicated in both the Matlab and Python simulations.

Testing has shown that the altitude command is being followed correctly.