First round of debugging IDP:

Fixed quaternions to be represented as (r,x,y,z) instead of (x,y,z,r)
Fixed a small mistake in EKF.createP()
Merged from last commit, fixed a small thing from A
Merged conflicts in EKF.java
Merged these dumb logs
Merged everything na hopefully
Merge branch 'master' of https://github.com/darkaeon10/EKFSLAM