Assuming that the sensor were mounted straight down, accounting for vehicle rotation would be as easy as 

```
AttitudeActualData attitude;
AttitudeActualGet(&attitude);

float Rbe[3][3];
Quaternion2R(&attitude.q1, Rbe);
range_body[3] = {sonar.range, 0, 0}; // Assuming the sensor is pointed straight down.
rot_mult(Rbe, range_body, range_earth, true);

range[NORTH] = range_earth[0];
range[EAST] = range_earth[1];
range[DOWN] = range_earth[2];
```

If we wanted to take into account sensor orientation, it would only look a little longer since we would first perform a rotation from sensor to body frame.

Note: I don't have a sensor so this isn't tested.