Trajectory Planning for Nonholonomic Mobile Robot Using Extended Kalman Filter

Algorithm 1

Computational method for distance 𝑑_𝑚 calculation using RF ToF.

input: The mobile robot is initialized with total number of active beacons

    in the environment (𝑁_𝑚) and theirs respectives address.

output: The distance between the robot and the beacon 𝑚 based at delay RF signal time.

System setup;

while The system is active  do

 for  𝑛=1  until  𝑛=𝑁_𝑚  do

   To transmit a frame-coded for beacon 𝑛;

   To initiate timer for period 𝑇 calculation;

   if  The same frame-coded signal returns.AND. 𝑡<100 ms  then

    Stops the timer and calculate the time 𝑇;

    Calculate the distance 𝑑_𝑚=0,3(𝑇−𝑡_𝑝𝑚−𝑡_𝑟𝑚−𝑡_𝑞)/2;

   else

    Some tramission/reception RF signal error occurred;

    Try next active beacon;

   end

 end

end