Updated the branch with the MATLAB codes and verified magnetometer if magnetometer is giving error. Not much