Direct Nonlinear Trajectory Optimization and State Estimation for a Tethered Underwater Energy Harvesting Kite