Fixed path ID output in state packets
Removed test/debugging code
Fixed NMPC stats output; added nav structure versioning support