diff --git a/class_model/include/class_model/FindLeader.h b/class_model/include/class_model/FindLeader.h
index 79f0a27..887c88d 100644
--- a/class_model/include/class_model/FindLeader.h
+++ b/class_model/include/class_model/FindLeader.h
@@ -2,10 +2,16 @@
 #include "class_model/DroneStatus.h"
 #include <iterator> 
 
-bool is_leader(global_location data[], size_t index){
+typedef struct result{
+    int leader_ID;
+    bool is_leader;
+}result;
+
+result check_leader(global_location data[], size_t index){
     
     float d[index];
-    int check_ID,leader_ID;
+    int check_ID;
+    result _result;
 
     for(int i=1 ; i<index ; i++){
 
@@ -17,14 +23,16 @@ bool is_leader(global_location data[], size_t index){
     std::vector<float> _d(d+1,d+index-1); 
     
     check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
-    leader_ID = check_ID + 1;
+    _result.leader_ID = check_ID + 1;
     
-    std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
+    std::cout <<"Drone " << _result.leader_ID << " is leader " << std::endl;
     
-    if (data[1].ID == leader_ID){
-        return 1;
+    if (data[1].ID == _result.leader_ID){
+        _result.is_leader = 1;
+        return _result;
     }else{
-        return 0;
+        _result.is_leader = 0;
+        return _result;
     }
     
 }
\ No newline at end of file
diff --git a/class_model/include/class_model/follower.h b/class_model/include/class_model/follower.h
index e3b92ef..1df97ce 100755
--- a/class_model/include/class_model/follower.h
+++ b/class_model/include/class_model/follower.h
@@ -24,7 +24,9 @@ public:
     DroneStatus drone1;
     DroneStatus drone2;    
     DroneStatus drone3;
-
+    DroneStatus self;
+    DroneStatus samePlane[3];
+    
     void follower();
 
     float lon[3],lat[3];
@@ -37,9 +39,12 @@ private:
     global_location target_location;
     global_location follower_location;
     global_location leader_location;
+    global_location leader_drone;
+    DroneStatus memberDrone[];
 
     void calculate_position(float k,float theta);
-    void plane(global_location target_location,global_location leader_location,global_location follower_location);
+    void plane(global_location target, global_location leader, global_location follower);
+    void find_ref_drone(global_location data[], size_t index, int leaderID);
     void choose_leader();
     void follow();
     void get_location();
diff --git a/class_model/src/bio_main.cpp b/class_model/src/bio_main.cpp
index 579c52c..441b2ea 100644
--- a/class_model/src/bio_main.cpp
+++ b/class_model/src/bio_main.cpp
@@ -26,8 +26,6 @@ int main(int argc, char **argv) {
     // SelectionClass selection_object;
 
     global_location target,self,M1,M2;
-    global_location pos[] = {target,self,M1,M2};
-    size_t index = sizeof(pos)/sizeof(pos[0]);
     
     YAML::Node config = YAML::LoadFile("/home/dodo/ardupilot_ws/src/class_model/src/config.yaml");  //reading yaml parameter(target point)
     target.lon = config["routh1"]["x"].as<float>();
@@ -37,10 +35,13 @@ int main(int argc, char **argv) {
     M1 = request_object.get_M1_GPS();
     M2 = request_object.get_M2_GPS();
 
-    if(is_leader(pos,index) == 1){
+    global_location pos[] = {target,self,M1,M2};
+    size_t index = sizeof(pos)/sizeof(pos[0]);
+
+    if(check_leader(pos,index).is_leader == 1){
         //fly2target
     }else{
-        //find reference drone
+        //find reference drone(pos,index,leader_ID);
     }
 
 
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index 6a38a01..c58fea5 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -14,24 +14,146 @@ void FollowerClass::follower(){
 
 }
 
-void FollowerClass::plane(global_location target_location,global_location leader_location,global_location follower_location){
+void FollowerClass::plane(global_location target, global_location leader, global_location follower){
     
     //caculate drone is locate on R/L plane
     float slope,c,y;
     slope = (target_location.lat-leader_location.lat)/(target_location.lon-leader_location.lon);
     c = target_location.lat - slope*target_location.lon;
-    y = slope * follower_location.lon + c;
-
-    if(follower_location.lat >= y){
-        drone1.plane = 1;        //right plane
+    y = slope * follower.lon + c;
+
+    if(follower.lat > y){
+        switch (follower.ID){
+            case 1:
+                drone1.init_location.lat = follower.lat;
+                drone1.init_location.lon = follower.lon;
+                drone1.plane = -1;        //right plane
+                break;
+            case 2:
+                drone2.init_location.lat = follower.lat;
+                drone2.init_location.lon = follower.lon;
+                drone2.plane = -1;        
+                break;            
+            case 3:
+                drone3.init_location.lat = follower.lat;
+                drone3.init_location.lon = follower.lon;
+                drone3.plane = -1;        
+                break; 
+        }
     }else if(follower_location.lat < y){
-        drone1.plane = 0;        //left plane
+        switch (follower.ID){
+            case 1:
+                drone1.init_location.lat = follower.lat;
+                drone1.init_location.lon = follower.lon;
+                drone1.ID = follower.ID; 
+                drone1.plane = 1;        //left plane
+                break;
+            case 2:
+                drone2.init_location.lat = follower.lat;
+                drone2.init_location.lon = follower.lon;
+                drone2.plane = 1;        
+                break;            
+            case 3:
+                drone3.init_location.lat = follower.lat;
+                drone3.init_location.lon = follower.lon;
+                drone3.plane = 1;       
+                break; 
+    }
     }
     
-    choose_leader();
+    
 }
 
-void FollowerClass::choose_leader(){
+void FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
+
+    for(int i=1 ; i<index ; i++){                //check leader drone
+        if(leaderID = data[i].ID){
+            leader_drone = data[i];
+            switch(leaderID){
+                case 1:
+                    drone1.init_location.lat = data[i].lat;        
+                    drone1.init_location.lon = data[i].lon;
+                    drone1.ID = data[i].ID;
+                    drone1.plane = 0;               //center
+                    break;
+                case 2:
+                    drone2.init_location.lat = data[i].lat;
+                    drone2.init_location.lon = data[i].lon;
+                    drone2.ID = data[i].ID;
+                    drone2.plane = 0;
+                    break;
+                case 3:
+                    drone3.init_location.lat = data[i].lat;
+                    drone3.init_location.lon = data[i].lon;
+                    drone3.ID = data[i].ID;
+                    drone3.plane = 0;
+                    break;
+
+        }
+        }
+    }
+
+    for(int i=1 ; i<index ; i++){
+        if(data[i].ID != leader_drone.ID){          //caculate the plane of each drone
+            plane(data[0],leader_drone,data[i]);
+        }
+    }
+
+    int j=0,k=0;
+    if(drone1.ID == data[1].ID){                   //seperate self and other
+        self = drone1;
+    }else{
+        memberDrone[j] = drone1;
+        j+=1;
+    }
+    if(drone2.ID == data[1].ID){
+        self = drone2;
+    }else{
+        memberDrone[j] = drone2;
+        j+=1;
+    }
+    if(drone3.ID == data[1].ID){
+        self = drone1;
+    }else{
+        memberDrone[j] = drone3;
+        j+=1;
+    }
+
+    for(int i=0;i<index-1;i++){
+        if(memberDrone[i].plane == self.ID){
+            samePlane[k] = memberDrone[i];
+            k++;
+        }
+    }
+
+
+    int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
+    float d[s_index];
+    int check_ID;
+
+    for(int i=0 ; i<s_index ; i++){
+
+        d[i] = sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lat - samePlane[i].init_location.lat),2) );
+
+        // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+    }
+
+    std::vector<float> _d(d,d+s_index-1); 
+    
+    check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
+    
+    // std::cout <<"Drone " <<  " is leader " << std::endl;
+
+
+
+
+
+
+
+
+    
+
+    
 
 
 
@@ -46,42 +168,42 @@ void FollowerClass::get_location(){
 
 }
 
-void FollowerClass::calculate_position(float k,float theta){
+// void FollowerClass::calculate_position(float k,float theta){
 
-    theta = theta*3.14/180;
-    float phi = request_object.get_leader_heading()/100;
-    phi = phi*3.14/180;
-    leader_location=request_object.get_leader_GPS();
-    follower_location=GPS_object.gps_position();
+//     theta = theta*3.14/180;
+//     float phi = request_object.get_leader_heading()/100;
+//     phi = phi*3.14/180;
+//     leader_location=request_object.get_leader_GPS();
+//     follower_location=GPS_object.gps_position();
 
-    double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
-    float transfer[2][2]={
-                            cos(phi),-sin(phi),
-                            sin(phi),cos(phi)
-                        };
-    float Q[2]={k*sin(theta),k*cos(theta)};
-    float T[2]={1,-1};
+//     double Pf[]={},Pl[]={leader_location.lon,leader_location.lat};
+//     float transfer[2][2]={
+//                             cos(phi),-sin(phi),
+//                             sin(phi),cos(phi)
+//                         };
+//     float Q[2]={k*sin(theta),k*cos(theta)};
+//     float T[2]={1,-1};
 
-    Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
-    Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
+//     Pf[0]=(transfer[0][0]*Q[0]+transfer[0][1]*Q[1])*T[0]+Pl[0]/100;
+//     Pf[1]=(transfer[1][0]*Q[0]+transfer[1][1]*Q[1])*T[1]+Pl[1]/100;
     
-    float error_x = Pf[0] - (follower_location.lon*1e+5);
-    float error_y = Pf[1] - (follower_location.lat*1e+5);
-
-    if (error_x < 0.3 & error_x > -0.3){
-        error_x = 0;
-    }
-    if (error_y < 0.3 & error_y > -0.3){
-        error_y = 0;
-    }
+//     float error_x = Pf[0] - (follower_location.lon*1e+5);
+//     float error_y = Pf[1] - (follower_location.lat*1e+5);
+
+//     if (error_x < 0.3 & error_x > -0.3){
+//         error_x = 0;
+//     }
+//     if (error_y < 0.3 & error_y > -0.3){
+//         error_y = 0;
+//     }
     
-    // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
-    ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
-    ROS_INFO("%f,%f",Pf[0],Pf[1]);
-    ROS_INFO("%f,%f",error_x,error_y);
-    ROS_INFO("************************************");
+//     // ROS_INFO("%f,%f",leader_location.lon/100 ,leader_location.lat/100);
+//     ROS_INFO("%f,%f",follower_location.lon*1e+5 ,follower_location.lat*1e+5);
+//     ROS_INFO("%f,%f",Pf[0],Pf[1]);
+//     ROS_INFO("%f,%f",error_x,error_y);
+//     ROS_INFO("************************************");
     
-    command_object.fix_velocity(error_x,error_y,0,0,0.1);
-    // sleep(0.5);
+//     command_object.fix_velocity(error_x,error_y,0,0,0.1);
+//     // sleep(0.5);
 
-}
+// }