First version of Integral control - not working properly

Estimator error was measured; around 2000 for a reasonable pitch angle,
up to 10000 according to andy for a higher (but possible) angle