Robustified filter init / sequencing
Fixed printing in attitude control