NAV working now in sim.

changed PID scaling; new nav parameters,
NAV working now in sim.