Some corrections for handling twist transforms. Also corrected handling of boolean update vector when a transform is required to go from the sensor to the body frame