Fixed a few initialization problems
Integrated structure servoing