UWB Based unmanned indoor Human Following Robot using (PoZYX)

I am working on Follow me robot using Pozyx , basically the robot is a trolley with 2 drive and 2 free wheels like (2wd),

i was using 2 anchors mounted in front of robot and 1 tag which i keep with me, by calculating the distance from each anchor the direction is decided where to move like shown in code, but this is a very basic approach and results are not same as wanted,

if ((range.distance > 1500) || (range1.distance > 1500)) // distance is in mm
    {
      Stop();

    }
    else {

      int difference = ((range.distance) - (range1.distance));

      
      if ((difference < 250) && (difference > -250))
      {
        Forword();
      }
      if (difference > 250)
      {
       Left();
      }
      if (difference < -250)
      {
       Right();
      }

after this testing i read some research papers people are using trigonometric formulas to calculate the x,y quarantines like Pythagorean theorem, Then i implemented the formula, but i am only able to get the x,y coordinates only in front of anchors A1, and A2 not left and right (Picture attached),

i need help regarding how to implement these x,y coordinates on motor and calculating the angle that will use for turning the robot left and right,

void localization(float r1, float r0, float d, float x0, float y0, float x1,float y1){

  
  float a= ((r0*r0)-(r1*r1)+(d*d))/(2*d);
  float b=((r1*r1)-(r0*r0)+(d*d))/(2*d);
  float h= sqrt((r0*r0)-(a*a));

 
        float x2 = x0 + a * (x1 - x0) / d;
        float y2 = y0 - a * (y1 - y0) / d;

        float x3= x2+((h*(y1-y0))/d);
        float y3= y2+((h*(x1-x0))/d);


    Serial.print(x3);
    Serial.print(",");
    Serial.print(y3);
    Serial.println(".");
  
  }