Added predicted trajectories to the estimate-sat3d to do some debugging on that (instead of in CRS-planner), and it all seems to work fine (the error must be in mappings).