Fix/mppi reference trajectory segfault

[michschmid]

Fixes the segfault which makes the mppi controller always crash after a few seconds. /!\ should be tested on the drone/other computer before merging