Added Mahony AHRS algorithm
Added options to change AHRS parameters
Start development of Soft Iron matrix loading from config file