Compass debug

Fixed up rosparam call to make sure biases are being pulled in properly. Tested on Kingfisher. 
fixes, coming up.