Fix average lean angle calculation in startup.
Merge remote-tracking branch 'oliverlee/fullorderobs' into full_order_observer