One thing I noticed today is the ekf has errror states. I reported in this thread. Not sure how to fix this.