Kalman filter now debugged and working. Still needs tuning. Logger now working.