working on DCM
compass heading working
fixing update matrix