Fix trajectory test bugs, add more asserts and tweaks.