Abstract: In this paper, true geodetic coordinates of the detection point was derived using data provide by AHRS (attitude and heading reference system), RTK-DGPS and ultrasonic sensors, where position and attitude angles of the vehicle body, installation position offsets and angle offsets of ultrasonic sensors relative to the vehicle body were taken into consideration. The algorithm consisted of some coordinate transformation formula to obtain the detection points in the geodetic coordinates. Tests were conducted to examine the algorithm, including a straight line driving test on brick-scattered uneven road, which was used to verify the effectiveness of the correction model, and some field tests of tree canopies. Experimental results showed that the correction model (algorithm), which used the body attitude and position information, correctly transformed any point on the body into geodetic frame, and any point on the canopy detected by the ultrasonic sensor into geodetic frame, and ensured the true canopy shape could be measured regardless of body attitude changes and sensor installation offsets.