Coming in late but seeing no solutions offered yet, I'll take a stab at it:
Assume your vehicle is traveling obliquely toward the wall.
Assume you can find and measure the perpendicular distance to the wall.
Then you can calculate the travel distance from those two measurements:
Next we need to find the angle between the vehicle's heading (center-line) and the perpendicular. A rotating sensor that senses return-signal strength could probably give good results but is mechanically more complicated. Given that the ultrasonic sensors have a somewhat broad sensing angle, 3 or 5 fixed sensors (1 straight ahead and 1 or 2 to either side) may work well enough if you're just trying to avoid hitting the wall. In that case you could measure with all of the sensors, take the least distance measurement as 'p' and the known angle of that sensor as 'theta'.
What's left is to decide how to make the calculation and work with cosines. For the rotating sensor case, you'll have to find the angle of the sensor at the time of it's smallest reading, and look up in a table (or calculate) the cosine of that angle. For the multiple fixed sensors case, you already know the sensors' angles and therefore, their cosines.
If you have enough code memory it would easy to write your calculations in floating point, certainly for some quick and dirty testing. If not, or you need to speed up the calculations, you can do it in fixed point with less code. it's a little bit harder for the programmer but much easier and quicker for the MCU.