Kite figure 8

Stable state machine, takes position, heading, outputs angle error
stable to some wiggling