Migrate to doing localization in 3D

[oswinso]

#629 is going to require us to do localization in 3D (When we're on the ramp, the projections are going to be wrong if we assume we're always at z=0 with pitch = 0).

While #555 is probably going to result in being able to perform 3D localization, to break a hard dependency with that issue I think we should look at the simpler method of using the current robot_localization ekf package to do localization in 3D.

The main things that need to be done will probably be:

• Turning the 2d_mode parameter to false (we currently have it set to true)
• Checking that we're using the roll and pitch of the imu (check the "matrix" of bools)
• Tune some parameters or something / figure out what the issues are in terms of being able to get a "decent" 3D localization working.

AC: Working 3D localization in simulation.