Distance measurement algorithm of the four ultrasonic sensors array

% Data availability in the communication channel

If data available continue; end;

If not, wait for more data.

% Data validity is expressed through the condition that the reading should be between

10 cm and 1000 cm

If data < 10 || data > 1000

then data valid, continue; end;

% Addition of channel selected as they are changing since the system is using 4 channels

(4 ultrasonic sensors) as organized by a multiplexer

counter (channel_ID) = counter (channel_ID) + 1;

% Local reflection point position where only the Z component (depth) is read while X

and Y (platform displacements) are set to zeros here

x = zeros(size(d));

y = zeros(size(d));

z = (0.5*d-p./(2*d));

where d = ultrasonic sensor distance

p = coordinates position of the ultrasonic sensor in the array with respect to the active

ultrasonic sensor