diff --git a/class_model/.vscode/launch.json b/class_model/.vscode/launch.json
index b9471b3..d87a6ba 100644
--- a/class_model/.vscode/launch.json
+++ b/class_model/.vscode/launch.json
@@ -9,7 +9,7 @@
             "name": "ROS: Launch",
             "type": "ros",
             "request": "launch",
-            "target": "/home/dodo/ardupilot_ws/src/class_model/launch/test.launch",
+            "target": "/home/dodo/ardupilot_ws/src/class_model/launch/testLib.launch",
             "launch": [
                 "rviz",
                 "gz",
diff --git a/class_model/include/class_model/FindLeader.h b/class_model/include/class_model/FindLeader.h
index 887c88d..f76cb81 100644
--- a/class_model/include/class_model/FindLeader.h
+++ b/class_model/include/class_model/FindLeader.h
@@ -20,7 +20,7 @@ result check_leader(global_location data[], size_t index){
         // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
     }
 
-    std::vector<float> _d(d+1,d+index-1); 
+    std::vector<float> _d(d+1,d+index); 
     
     check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     _result.leader_ID = check_ID + 1;
diff --git a/class_model/src/.vscode/c_cpp_properties.json b/class_model/src/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..cf0dd9b
--- /dev/null
+++ b/class_model/src/.vscode/c_cpp_properties.json
@@ -0,0 +1,24 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/dodo/ardupilot_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/dodo/ardupilot_ws/src/ROS_controll/include/**",
+        "/home/dodo/ardupilot_ws/src/class_model/include/**",
+        "/home/dodo/ardupilot_ws/src/iq_gnc/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file
diff --git a/class_model/src/.vscode/settings.json b/class_model/src/.vscode/settings.json
new file mode 100644
index 0000000..c3c2f5b
--- /dev/null
+++ b/class_model/src/.vscode/settings.json
@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/dodo/ardupilot_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/dodo/ardupilot_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}
\ No newline at end of file
diff --git a/class_model/src/follower.cpp b/class_model/src/follower.cpp
index 5134614..7240d76 100755
--- a/class_model/src/follower.cpp
+++ b/class_model/src/follower.cpp
@@ -22,21 +22,24 @@ void FollowerClass::plane(global_location target, global_location leader, global
     c = target.lat - slope*target.lon;
     y = slope * follower.lon + c;
 
-    if(follower.lat > y){
+    if(follower.lat >= y){               
         switch (follower.ID){
             case 1:
                 drone1.init_location.lat = follower.lat;
                 drone1.init_location.lon = follower.lon;
+                drone1.ID = follower.ID; 
                 drone1.plane = -1;        //right plane
                 break;
             case 2:
                 drone2.init_location.lat = follower.lat;
                 drone2.init_location.lon = follower.lon;
+                drone2.ID = follower.ID; 
                 drone2.plane = -1;        
                 break;            
             case 3:
                 drone3.init_location.lat = follower.lat;
                 drone3.init_location.lon = follower.lon;
+                drone3.ID = follower.ID; 
                 drone3.plane = -1;        
                 break; 
         }
@@ -51,11 +54,13 @@ void FollowerClass::plane(global_location target, global_location leader, global
             case 2:
                 drone2.init_location.lat = follower.lat;
                 drone2.init_location.lon = follower.lon;
+                drone2.ID = follower.ID; 
                 drone2.plane = 1;        
                 break;            
             case 3:
                 drone3.init_location.lat = follower.lat;
                 drone3.init_location.lon = follower.lon;
+                drone3.ID = follower.ID; 
                 drone3.plane = 1;       
                 break; 
     }
@@ -68,10 +73,10 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
 
     global_location vector_LT,vector_SM;
     float m,n,k,cosTheta;
-    vector_LT.lon = leader.lon - target.lon;       //vector_LT (leader->target)
-    vector_LT.lat = leader.lat - target.lat;
-    vector_SM.lon = leader.lon - target.lon;       //vector_SM (self->member)
-    vector_SM.lat = leader.lat - target.lat;
+    vector_LT.lon = target.lon - leader.lon;       //vector_LT (leader->target)     
+    vector_LT.lat = target.lat - leader.lat;
+    vector_SM.lon = member.init_location.lon - self.init_location.lon;       //vector_SM (self->member)
+    vector_SM.lat = member.init_location.lat - self.init_location.lat;
     k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat);     //k = vector_LT dot vector_SM
     m = sqrt(pow(vector_LT.lon,2)+pow(vector_LT.lat,2));     //m = |vector_LT|
     n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2));     //n = |vector_SM|
@@ -80,7 +85,7 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
     if(cosTheta > 0){
         return 1;
     }else{
-        return INFINITY;
+        return 10000;
     }
 
 }
@@ -88,7 +93,7 @@ int direct(global_location target, global_location leader, DroneStatus self, Dro
 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){
+        if(leaderID == data[i].ID){
             leader_drone = data[i];
             switch(leaderID){
                 case 1:
@@ -111,6 +116,7 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
                     break;
 
         }
+        // std::cout << data[i].ID << std::endl;
         }
     }
 
@@ -120,52 +126,59 @@ void FollowerClass::find_ref_drone(global_location data[], size_t index, int lea
         }
     }
 
-    int j=0,k=0;
+    int j=0,s_index=0;
     if(drone1.ID == data[1].ID){                   //seperate self and other
-        self = drone1;
+        self = drone1;                              //problem
     }else{
         memberDrone[j] = drone1;
         j+=1;
+        std::cout << "drone1" <<std::endl;
     }
     if(drone2.ID == data[1].ID){
         self = drone2;
     }else{
         memberDrone[j] = drone2;
         j+=1;
+        std::cout << "drone2" <<std::endl;
     }
     if(drone3.ID == data[1].ID){
-        self = drone1;
+        self = drone3;
     }else{
         memberDrone[j] = drone3;
         j+=1;
+        std::cout << "drone3" <<std::endl;
     }
 
-    for(int i=0;i<index-1;i++){
-        if(memberDrone[i].plane == self.plane){    //load same plane member
-            samePlane[k] = memberDrone[i];
-            k++;
+    for(int i=0;i<index-2;i++){
+        if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){    //load same plane member
+            samePlane[s_index] = memberDrone[i];
+            std::cout << samePlane[s_index].init_location.lat <<std::endl;
+            std::cout << memberDrone[i].ID <<std::endl;
+            s_index++;
         }
+
     }
 
 
-    int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
+    // int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
     float d[s_index];
     int dir,check_index,ref_ID;
-
+    std::cout <<"s_index " <<s_index <<std::endl;
     for(int i=0 ; i<s_index ; i++){                 //caculate each same plane mamber's distance
 
         dir = direct(data[0],leader_drone,self,samePlane[i]);
-        d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lat - samePlane[i].init_location.lat),2) );
+        d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
 
-        // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+        std::cout << i<<"," <<d[i] <<std::endl;
+        std::cout << samePlane[i].ID <<std::endl;
     }
 
-    std::vector<float> _d(d,d+s_index-1); 
+    std::vector<float> _d(d,d+s_index); 
     
     check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
     ref_ID = samePlane[check_index].ID;
 
-    // std::cout <<"Drone " <<  " is leader " << std::endl;    
+    std::cout <<"refID " <<  ref_ID << std::endl;   
 
 }
 
diff --git a/class_model/src/requestData.cpp b/class_model/src/requestData.cpp
index 31c8c46..1778bcf 100755
--- a/class_model/src/requestData.cpp
+++ b/class_model/src/requestData.cpp
@@ -7,19 +7,19 @@ int command;
 
 RequestClass::RequestClass() : node_handle_(""){
 
-  // mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
-  //                                  &RequestClass::Data_callback, this);
+  mqtt_data = node_handle_.subscribe("/Flight_Information_reciver", 100,
+                                   &RequestClass::Data_callback, this);
   formation_data = node_handle_.subscribe("/Fly_Formation_reciver", 10,
                                    &RequestClass::Message_callback, this);
-  mqtt_data = node_handle_.subscribe("/uav_message", 100,
-                                   &RequestClass::Data_callback, this);
+  // mqtt_data = node_handle_.subscribe("/uav_message", 100,
+  //                                  &RequestClass::Data_callback, this);
 
-  self_data = node_handle_.subscribe("/self_data_message", 100,
-                                   &RequestClass::self_callback, this);       //test bionic
-  member1_data = node_handle_.subscribe("/member1_data_message", 100,
-                                   &RequestClass::M1_callback, this);
-  member2_data = node_handle_.subscribe("/member2_data_message", 100,
-                                   &RequestClass::M2_callback, this);
+  // self_data = node_handle_.subscribe("/self_data_message", 100,
+  //                                  &RequestClass::self_callback, this);       //test bionic
+  // member1_data = node_handle_.subscribe("/member1_data_message", 100,
+  //                                  &RequestClass::M1_callback, this);
+  // member2_data = node_handle_.subscribe("/member2_data_message", 100,
+  //                                  &RequestClass::M2_callback, this);
 }
 
 RequestClass::~RequestClass() { ros::shutdown(); }
@@ -92,50 +92,50 @@ void RequestClass::L_INFO(std::string data){
 }
 
 /*test biomic*/
-void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
-  Bio_INFO(sensor->data);
-}
-void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
-  Bio_INFO(sensor->data);
-}
-void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
-  Bio_INFO(sensor->data);
-}
-
-void RequestClass::Bio_INFO(std::string data){
-
-  j_data = json::parse(data);        //open source
-
-  drone_ID = j_data["ID"];
-  std::remove(ID.begin(),ID.end(),self_ID);
-
-  if (drone_ID == self_ID){
-    self_position.lat=j_data["lat"];   
-	  self_position.lon=j_data["lon"];
-    self_position.alt=j_data["alt"];
-    self_position.heading = j_data["heading"];
-  }else if (drone_ID == ID[0]){
-    M1_position.lat=j_data["lat"];   
-	  M1_position.lon=j_data["lon"];
-    M1_position.alt=j_data["alt"];
-    M1_position.heading = j_data["heading"];
-  }else if (drone_ID == ID[1]){
-    M2_position.lat=j_data["lat"];   
-	  M2_position.lon=j_data["lon"];
-    M2_position.alt=j_data["alt"];
-    M2_position.heading = j_data["heading"];
-  }
-
-}
-global_location RequestClass::get_self_GPS(){
-
-  return self_position; 
-}
-global_location RequestClass::get_M1_GPS(){
-
-  return M1_position; 
-}
-global_location RequestClass::get_M2_GPS(){
-
-  return M2_position; 
-}
\ No newline at end of file
+// void RequestClass::self_callback(const std_msgs::String::ConstPtr &sensor) {
+//   Bio_INFO(sensor->data);
+// }
+// void RequestClass::M1_callback(const std_msgs::String::ConstPtr &sensor) {
+//   Bio_INFO(sensor->data);
+// }
+// void RequestClass::M2_callback(const std_msgs::String::ConstPtr &sensor) {
+//   Bio_INFO(sensor->data);
+// }
+
+// void RequestClass::Bio_INFO(std::string data){
+
+//   j_data = json::parse(data);        //open source
+
+//   drone_ID = j_data["ID"];
+//   std::remove(ID.begin(),ID.end(),self_ID);
+
+//   if (drone_ID == self_ID){
+//     self_position.lat=j_data["lat"];   
+// 	  self_position.lon=j_data["lon"];
+//     self_position.alt=j_data["alt"];
+//     self_position.heading = j_data["heading"];
+//   }else if (drone_ID == ID[0]){
+//     M1_position.lat=j_data["lat"];   
+// 	  M1_position.lon=j_data["lon"];
+//     M1_position.alt=j_data["alt"];
+//     M1_position.heading = j_data["heading"];
+//   }else if (drone_ID == ID[1]){
+//     M2_position.lat=j_data["lat"];   
+// 	  M2_position.lon=j_data["lon"];
+//     M2_position.alt=j_data["alt"];
+//     M2_position.heading = j_data["heading"];
+//   }
+
+// }
+// global_location RequestClass::get_self_GPS(){
+
+//   return self_position; 
+// }
+// global_location RequestClass::get_M1_GPS(){
+
+//   return M1_position; 
+// }
+// global_location RequestClass::get_M2_GPS(){
+
+//   return M2_position; 
+// }
\ No newline at end of file
diff --git a/class_model/src/test_lib.cpp b/class_model/src/test_lib.cpp
index 3ccfa0a..b1926bc 100644
--- a/class_model/src/test_lib.cpp
+++ b/class_model/src/test_lib.cpp
@@ -3,37 +3,221 @@
 #include "class_model/gps_location.h"
 #include <algorithm>
 #include <math.h>
+#include "class_model/DroneStatus.h"
 
 using namespace std;
+    global_location leader_drone;
+    DroneStatus memberDrone[3];
+    DroneStatus drone1;
+    DroneStatus drone2;    
+    DroneStatus drone3;
+    DroneStatus self;
+    DroneStatus samePlane[3];
+    global_location follower_location;
 
-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){
     
     // int index = sizeof(data)/sizeof(data[0]);
     float d[index];
-    int check_ID,leader_ID;
+    int check_ID;
+    result _result;
 
     for(int i=1 ; i<index ; i++){
 
         d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
 
-        // std::cout << i<<"," <<d[data[i].ID] <<std::endl;
+        std::cout << i<<"," <<d[data[i].ID] <<std::endl;
     }
 
-    std::vector<float> _d(d+1,d+index-1); 
+    std::vector<float> _d(d+1,d+index); 
     
     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 << _d[0] << " ,"<< _d[1] << " ,"<< _d[2] << 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;
+    }
+    
+}
+void plane(global_location target, global_location leader, global_location follower){
+    
+    //caculate drone is locate on R/L plane
+    float slope,c,y;
+    slope = (target.lat-leader.lat)/(target.lon-leader.lon);
+    c = target.lat - slope*target.lon;
+    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.ID = follower.ID; 
+                drone1.plane = -1;        //right plane
+                break;
+            case 2:
+                drone2.init_location.lat = follower.lat;
+                drone2.init_location.lon = follower.lon;
+                drone2.ID = follower.ID; 
+                drone2.plane = -1;        
+                break;            
+            case 3:
+                drone3.init_location.lat = follower.lat;
+                drone3.init_location.lon = follower.lon;
+                drone3.ID = follower.ID; 
+                drone3.plane = -1;        
+                break; 
+        }
+    }else if(follower_location.lat < y){
+        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.ID = follower.ID; 
+                drone2.plane = 1;        
+                break;            
+            case 3:
+                drone3.init_location.lat = follower.lat;
+                drone3.init_location.lon = follower.lon;
+                drone3.ID = follower.ID; 
+                drone3.plane = 1;       
+                break; 
+    }
     }
     
+    
+}
+
+int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
+
+    global_location vector_LT,vector_SM;
+    float m,n,k,cosTheta;
+    vector_LT.lon = target.lon - leader.lon;       //vector_LT (leader->target)     
+    vector_LT.lat = target.lat - leader.lat;
+    vector_SM.lon = member.init_location.lon - self.init_location.lon;       //vector_SM (self->member)
+    vector_SM.lat = member.init_location.lat - self.init_location.lat;
+    k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat);     //k = vector_LT dot vector_SM
+    m = sqrt(pow(vector_LT.lon,2)+pow(vector_LT.lat,2));     //m = |vector_LT|
+    n = sqrt(pow(vector_SM.lon,2)+pow(vector_SM.lat,2));     //n = |vector_SM|
+    cosTheta = k / (m*n);
+
+    if(cosTheta > 0){
+        return 1;
+    }else{
+        return 10000;
+    }
+
 }
 
+void 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;
+
+        }
+        // std::cout << data[i].ID << std::endl;
+        }
+    }
+
+    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,s_index=0;
+    if(drone1.ID == data[1].ID){                   //seperate self and other
+        self = drone1;                              //problem
+    }else{
+        memberDrone[j] = drone1;
+        j+=1;
+        std::cout << "drone1" <<std::endl;
+    }
+    if(drone2.ID == data[1].ID){
+        self = drone2;
+    }else{
+        memberDrone[j] = drone2;
+        j+=1;
+        std::cout << "drone2" <<std::endl;
+    }
+    if(drone3.ID == data[1].ID){
+        self = drone3;
+    }else{
+        memberDrone[j] = drone3;
+        j+=1;
+        std::cout << "drone3" <<std::endl;
+    }
+
+    for(int i=0;i<index-2;i++){
+        if(memberDrone[i].plane == self.plane || memberDrone[i].plane == 0){    //load same plane member
+            samePlane[s_index] = memberDrone[i];
+            std::cout << samePlane[s_index].init_location.lat <<std::endl;
+            std::cout << memberDrone[i].ID <<std::endl;
+            s_index++;
+        }
+
+    }
+
+
+    // int s_index = sizeof(samePlane)/sizeof(samePlane[0]);
+    float d[s_index];
+    int dir,check_index,ref_ID;
+    std::cout <<"s_index " <<s_index <<std::endl;
+    for(int i=0 ; i<s_index ; i++){                 //caculate each same plane mamber's distance
+
+        dir = direct(data[0],leader_drone,self,samePlane[i]);
+        d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
+
+        std::cout << i<<"," <<d[i] <<std::endl;
+        std::cout << samePlane[i].ID <<std::endl;
+    }
+
+    std::vector<float> _d(d,d+s_index); 
+    
+    check_index = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));     //get minimum value's index
+    ref_ID = samePlane[check_index].ID;
+
+    std::cout <<"refID " <<  ref_ID << std::endl;    
+
+}
 
 
 
@@ -59,39 +243,25 @@ int main(int argc,char** argv){
     target.lat = 0;
     target.heading = 0;
     
-    self.lat = 1;
-    self.lon = 1;
+    self.lat = 2;
+    self.lon = 2;
     self.ID = 1;
 
-    M1.lat = 2;
-    M1.lon = 2;
+    M1.lat = 1;
+    M1.lon = 1;
     M1.ID = 2;
 
-    M2.lat = 6;
-    M2.lon = 6;
+    M2.lat = 5;
+    M2.lon = 5;
     M2.ID = 3;
 
     global_location data[]={target,self,M1,M2};
+    size_t index = sizeof(data)/sizeof(data[0]);
 
 
-    std::cout << is_leader(data,sizeof(data)/sizeof(data[0])) <<std::endl;
-
-    // int index = sizeof(data)/sizeof(target); //4
-    // float d[index];
-    // int check_ID,leader_ID;
-
-    // for(int i=1 ; i<index ; i++){
-
-    //     d[data[i].ID] = sqrt( pow((data[0].lat - data[i].lat),2) + pow((data[0].lon - data[i].lon),2) );
-
-    //     std::cout << i<<"," <<d[data[i].ID] <<std::endl;
-    // }
+    find_ref_drone(data,index,check_leader(data,index).leader_ID);
 
-    // std::vector<float> _d(d+1,d+index-1); 
     
-    // check_ID = std::distance(_d.begin(),std::min_element(_d.begin(),_d.end()));
-    // leader_ID = check_ID + 1;
-    // std::cout <<"Drone " << leader_ID << " is leader " << std::endl;
 
 
     return 0;
diff --git a/iq_sim/worlds/three_drone.world b/iq_sim/worlds/three_drone.world
new file mode 100644
index 0000000..7c03902
--- /dev/null
+++ b/iq_sim/worlds/three_drone.world
@@ -0,0 +1,186 @@
+<?xml version="1.0"?> 
+<sdf version="1.5">
+  <world name="default">
+    <physics type="ode">
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>100</iters>
+          <sor>1.0</sor>
+        </solver>
+        <constraints>
+          <cfm>0.0</cfm>
+          <erp>0.9</erp>
+          <contact_max_correcting_vel>0.1</contact_max_correcting_vel>
+          <contact_surface_layer>0.0</contact_surface_layer>
+        </constraints>
+      </ode>
+      <real_time_update_rate>-1</real_time_update_rate>
+    </physics>
+  
+  <scene>
+    <ambient>0.0 0.0 0.0 1.0</ambient>
+    <shadows>0</shadows>
+  </scene>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>5000 5000</size>
+            </plane>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1</mu>
+                <mu2>1</mu2>
+              </ode>
+            </friction>
+          </surface>
+        </collision>
+        <visual name="runway">
+          <pose>000 0 0.005 0 0 0</pose>
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>1829 45</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Runway</name>
+            </script>
+          </material>
+        </visual>
+
+        <visual name="grass">
+          <pose>0 0 -0.1 0 0 0</pose>
+          <cast_shadows>false</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>5000 5000</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grass</name>
+            </script>
+          </material>
+        </visual>
+
+      </link>
+    </model>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <model name="drone1">
+      <pose> 0 0 0 0 0 0</pose>
+      <include>
+        <uri>model://drone1</uri>
+      </include>
+    </model>
+    <model name="drone2">
+      <pose> 0 5 0 0 0 0</pose>
+      <include>
+        <uri>model://drone2</uri>
+      </include>
+    </model>
+
+    <model name="drone3">
+      <pose> 0 -5 0 0 0 0</pose>
+      <include>
+        <uri>model://drone3</uri>
+      </include>
+    </model>
+
+<!--     <model name="tree">
+      <pose> 5 -10 0 0 0 3.14</pose>
+      <include>
+        <uri>model://oak_tree</uri>
+      </include>
+    </model>  -->
+
+    <plugin name='wind_plugin' filename='libgazebo_wind_plugin.so'>
+      <frameId>base_link</frameId>
+      <robotNamespace/>
+      <windVelocityMean>4.0</windVelocityMean>
+      <windVelocityMax>20.0</windVelocityMax>
+      <windVelocityVariance>0</windVelocityVariance>
+      <windDirectionMean>0 1 0</windDirectionMean>
+      <windDirectionVariance>0</windDirectionVariance>
+      <windGustStart>0</windGustStart>
+      <windGustDuration>0</windGustDuration>
+      <windGustVelocityMean>0</windGustVelocityMean>
+      <windGustVelocityMax>20.0</windGustVelocityMax>
+      <windGustVelocityVariance>0</windGustVelocityVariance>
+      <windGustDirectionMean>1 0 0</windGustDirectionMean>
+      <windGustDirectionVariance>0</windGustDirectionVariance>
+      <windPubTopic>world_wind</windPubTopic>
+    </plugin>    
+    <!--<model name="drone4">
+      <pose> 5 0 0 0 0 4.71</pose>
+      <include>
+        <uri>model://drone4</uri>
+      </include>
+    </model>
+     <model name="drone5">
+      <pose> 6 0 0 0 0 0</pose>
+      <include>
+        <uri>model://drone5</uri>
+      </include>
+    </model>
+    <model name="drone6">
+      <pose> 6 2 0 0 0 0</pose>
+      <include>
+        <uri>model://drone6</uri>
+      </include>
+    </model>
+    <model name="drone7">
+      <pose> 8 0 0 0 0 0</pose>
+      <include>
+        <uri>model://drone7</uri>
+      </include>
+    </model>
+    <model name="drone8">
+      <pose> 8 2 0 0 0 0</pose>
+      <include>
+        <uri>model://drone8</uri>
+      </include>
+    </model>
+    <model name="drone9">
+      <pose> 12 0 0 0 0 0</pose>
+      <include>
+        <uri>model://drone9</uri>
+      </include>
+    </model>
+    <model name="drone10">
+      <pose> 12 2 0 0 0 0</pose>
+      <include>
+        <uri>model://drone10</uri>
+      </include>
+    </model>
+    <model name="drone11">
+      <pose> 14 0 0 0 0 0</pose>
+      <include>
+        <uri>model://drone11</uri>
+      </include>
+    </model>
+    <model name="drone12">
+      <pose> 14 2 0 0 0 0</pose>
+      <include>
+        <uri>model://drone12</uri>
+      </include>
+    </model> -->
+
+  </world>
+</sdf>
+