How to locate the unknown node's position through the hello messages sent from the anchor nodes?

 

[Background]

1. Refer to http://en.wikipedia.org/wiki/Trilateration first.

2. The anchor node is the node that knows its position.

3. The unknown node is the node that does not know its position.

4. The anchor node will periodically broadcast the hello message which contains its position information in it.

5. The unknown node will receive the hello message and calculate the distance between the anchor node and itself based on the received signal strength. When the unknown nodes gets more than or equal to 3 anchor nodes' information, it will use the numerical analysis method to calculate its own position.

 

[Preparations]

1. the anchor node's program (pos_s.cc) : Put this file under /root/NCTUns-6.0/tools/tacticMANET/lib/

#include <stdio.h>

#include <signal.h>

#include <stdlib.h>

#include <unistd.h>

#include <sys/time.h>

#include <math.h>

#include "tactic_api.h"

#include <sys/types.h>

#include <sys/socket.h>

#include <netinet/in.h>

#include <arpa/inet.h>

#include <string.h>

#include <netdb.h>

 

int mynid, n, myTCPsockfd;

double curX, curY, curZ;

int socketfd2;

 

//structure for hello message

struct sent_info

{

  int nid;  // anchor's node id

  double x; // anchor's node x coordinate

  double y; // anchor's node y coordinate

  double z; // anchor's node z coordinate

  double str; // received signal strength. Initally 0, when received, write the RSS on it.

};

 

 

int main(int argc, char *argv[])

{

    int sockfd;

    struct sockaddr_in servaddr;    

    struct sent_info sinfo;

    int numbytes;

    char buf[100];

   

    //prepare a socket for broadcast out the hello message 

    if((sockfd=socket(AF_INET, SOCK_DGRAM, 0))==-1) {

      perror("socket");

      exit(1);

    } 

 

    bzero(&servaddr, sizeof(servaddr));

    servaddr.sin_family = AF_INET;

    servaddr.sin_port = htons(8888); // port is set to 8888

    //set the broadcast ip address

    if(inet_pton(AF_INET, "1.0.1.255", &servaddr.sin_addr)<=0)

    {

      perror("It is not a valid IP address\n");

      exit(1);

    }

 

    //communicate with the simulation engine to get the anchor node's information

    //get the node id

    mynid = getMyNodeID();

    myTCPsockfd = createTCPSocketForCommunicationWithSimulationEngine(

         mynid, 1, -1, -1, -1, -1, -1, -1, -1, 0, socketfd2, PROCESS_TYPE_AGENT, 1);

    constructGridMapofTheWholeField();

   

    //get the node's position

    n = getInitialNodePosition(mynid, curX, curY, curZ);

    printf("mynid:%d, my position x: %f, y: %f z: %f\n", mynid, curX, curY, curZ);

   

    //write the information to corresponding field to prepare the hello message

    sinfo.nid=mynid;

    sinfo.x=curX;

    sinfo.y=curY;

    sinfo.z=curZ;

    sinfo.str=0.0;

    memcpy(buf, &sinfo, sizeof(struct sent_info));

  

    //send out the first hello message

    if((numbytes=sendto(sockfd, buf, 100, 0, (struct sockaddr *)&servaddr, sizeof(servaddr)))==-1) {

      perror("sendto");

      exit(1);

    }

  

    //release the CPU control

    usleepAndReleaseCPU(myTCPsockfd, mynid, 1, 1);

  

    //periodically send out the hello message

    while (1) {

      n = getCurrentPosition(myTCPsockfd, mynid, curX, curY, curZ, 1);

     

      sinfo.nid=mynid;

      sinfo.x=curX;

      sinfo.y=curY;

      sinfo.z=curZ;

      sinfo.str=0.0;

      memcpy(buf, &sinfo, sizeof(struct sent_info));

  

      if((numbytes=sendto(sockfd, buf, 100, 0, (struct sockaddr *)&servaddr, sizeof(servaddr)))==-1) {

        perror("sendto");

        exit(1);

      }

        

      usleepAndReleaseCPU(myTCPsockfd, mynid, 300000, 1);

    }

}

 

2. the unknown node's program (pos_c.cc): Put this file under /root/NCTUns-6.0/tools/tacticMANET/lib/

#include <stdio.h>

#include <signal.h>

#include <stdlib.h>

#include <unistd.h>

#include <sys/time.h>

#include <math.h>

#include "tactic_api.h"

#include <sys/types.h>

#include <sys/socket.h>

#include <netinet/in.h>

#include <arpa/inet.h>

#include <string.h>

#include <netdb.h>

#include <fcntl.h>

 

//light speed

#define vel      3e8

 

int mynid, n, myTCPsockfd;

double curX, curY, curZ;

int socketfd2;

 

//structure for hello message

struct sent_info

{

  int nid;  // anchor's node id

  double x; // anchor's node x coordinate

  double y; // anchor's node y coordinate

  double z; // anchor's node z coordinate

  double str; // received signal strength. Initally 0, when received, write the RSS on it.

};

 

//anchor's node information

struct ref_node

{

  int nid;   //anchor node's id

  double x;  //anchor's node x coordinate

  double y;  //anchor's node y coordinate

  double z;  //anchor's node z coordinate

  double dist; //estimated distance from itself to the anchor node

  double err;  //error used in the numerical analysis

  double time; //the time when received the hello message

};

 

int main(int argc, char *argv[])

{

    int sockfd;

    struct sockaddr_in servaddr, cliaddr;    

    struct sent_info sinfo;                                                   

    int numbytes;

    char buf[100];

    socklen_t len;

    int flags;

    double dist, esti_dist=0.0;

    double freq=2400000000.0; //Hz

    double Lambda=vel/freq;  

    int i, j, k;

    struct timeval now;

    struct ref_node  ref[3];

    int found=0, set, old_index;

    double old_time;

 

    //reset the anchnor nodes' information

    for(k=0; k<3; k++)                                                               

    {

      ref[k].nid=-1;

      ref[k].x=-1.0;

      ref[k].y=-1.0;

      ref[k].z=-1.0;

      ref[k].dist=-1.0;

      ref[k].err=-1.0;

      ref[k].time=-1.0;

    }

 

    //prepare a socket to receive the hello message

    if((sockfd=socket(AF_INET, SOCK_DGRAM, 0))==-1) {

      perror("socket");

      exit(1);

    } 

   

    //set the nonblocking mode for this socket

    flags=fcntl(sockfd, F_GETFL);

    flags|=O_NONBLOCK;

    fcntl(sockfd, F_SETFL, flags);

 

    bzero(&servaddr, sizeof(servaddr));

    servaddr.sin_family = AF_INET;

    servaddr.sin_port = htons(8888); //port is set to 8888

    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);

 

    if(bind(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr))==-1) {

      perror("bind");

      exit(1);

    }

 

    //communicate with the simulation engine to know its real position

    //In order to calculate the calcution error, the real position must be known

    mynid = getMyNodeID();

    myTCPsockfd = createTCPSocketForCommunicationWithSimulationEngine(

         mynid, 1, -1, -1, -1, -1, -1, -1, -1, 0, socketfd2, PROCESS_TYPE_AGENT, 1);

    constructGridMapofTheWholeField();

    n = getInitialNodePosition(mynid, curX, curY, curZ);

    printf("mynid:%d, my position x: %f, y: %f z: %f\n", mynid, curX, curY, curZ);

  

    usleepAndReleaseCPU(myTCPsockfd, mynid, 1, 1);

  

    //continuously receive the hello message and estimate its position

    while (1) {

      n = getCurrentPosition(myTCPsockfd, mynid, curX, curY, curZ, 1);

      //printf("mynid:%d, my position x:%f, y:%f z:%f\n", mynid, curX, curY, curZ);

     

      //receive the hello message        

      len = sizeof(cliaddr);

      numbytes=recvfrom(sockfd, buf, 100, 0, (struct sockaddr *)&cliaddr, &len);

      memcpy(&sinfo, buf, sizeof(sinfo));

     

      //the real distance between the unknown node and the anchor node

      dist=sqrt(pow(curX-sinfo.x, 2)+pow(curY-sinfo.y, 2)+pow(curZ-sinfo.z, 2));

     

      /* estimated distance

      In this example, we set the propagation model is free-space model.

      Therefore, we simply use this model to get the distance from the anchor node.

      In reality, the unknown node should prepare a function to get the relationship

      between the received singal strength and distance.

      */     

      esti_dist=sqrt((pow(10, 1.5)*Lambda*Lambda)/(4*4*3.14159*3.14159*pow(10, sinfo.str/10)));

     

      printf("mynid=%d sent_id:%d, sent position x=%f, y=%f, Pr:%f dbm, dist=%f m, esti_dist=%fm\n", mynid, sinfo.nid, sinfo.x, sinfo.y, sinfo.str, dist, esti_dist);

 

      //record the time for the received hello message

      gettimeofday(&now, 0);

    

      /*

      Here I only prepare three data records. When the three data records have been written

      and the received anchor node id is not in the records, the oldest record will be over-written.

      */

     

      //case 1: the anchor node's information is in the records

      found=0;

      for(k=0; k<3;k++)

      {

        if(ref[k].nid==sinfo.nid)

        {

          //record the anchor node's position information

          ref[k].x=sinfo.x;

          ref[k].y=sinfo.y;

          ref[k].z=sinfo.z;

          //esti_dist is for free space model

          ref[k].dist=esti_dist;

          ref[k].time=(double)now.tv_sec + (double)now.tv_usec/1000000.0;

          found=1;

          //printf("%f update record %d, nid=%d x=%f y=%f z=%f\n", ref[k].time, k, ref[k].nid, ref[k].x, ref[k].y, ref[k].z);

          break;

        }

       }

 

       //case 2: the anchor node's information is not in the records

       if(found==0)

       {

         old_time=ref[0].time;

         old_index=0;

         for(k=1;k<3;k++)

         {

            if(ref[k].time < old_time){

               old_time=ref[k].time;

               old_index=k;

            }

         }

         ref[old_index].nid=sinfo.nid;

         ref[old_index].x=sinfo.x;

         ref[old_index].y=sinfo.y;

         ref[old_index].z=sinfo.z;

         ref[old_index].dist=esti_dist;

         ref[old_index].time=(double)now.tv_sec + (double)now.tv_usec/1000000.0;        

         //printf("%f write record into index %d nid=%d x=%f y=%f z=%f\n", ref[old_index].time, old_index, ref[old_index].nid, ref[old_index].x, ref[old_index].y, ref[old_index].z);  

        }

 

      set=1;

      //Assumption: the simulation topology is set to 500 x 500 m

      //every possible x and y coordincate is tried to find the approximate position for the unkown node

      for(i=0;i<=500;i++)

      {

        for(j=0;j<=500;j++)

        {

          if(ref[0].nid>0)

            ref[0].err=sqrt(pow(ref[0].x-i, 2.0)+pow(ref[0].y-j, 2.0))-ref[0].dist;

               else {

            set=0;

            goto ABC;

          }

 

          if(ref[1].nid>0)

            ref[1].err=sqrt(pow(ref[1].x-i, 2.0)+pow(ref[1].y-j, 2.0))-ref[1].dist;

          else {

            set=0;

            goto ABC;

          }

 

          if(ref[2].nid>0)

            ref[2].err=sqrt(pow(ref[2].x-i, 2.0)+pow(ref[2].y-j, 2.0))-ref[2].dist;     

          else {

            set=0;

            goto ABC;

          }

 

          //if all the calcuated distance errors are less than 0.5, we get the answer.

          if(set && ref[0].err < 0.5 && ref[1].err < 0.5 && ref[2].err < 0.5)

            goto ABC;

        }

      }

ABC:     

      if(set)

        printf("real x=%f y=%f, estimated x=%d y=%d\n", curX, curY, i, j);

     

      usleepAndReleaseCPU(myTCPsockfd, mynid, 200000, 1);

    }

}

 

3. Compile the program and set the link.

1

 use vi to edit the Makefile

2

 

 recompile

3

 

set the link

4

5

 

4. Modify the NCTUNS source to record the received signal strength when the unknown node receives the hello message.

(Here I modify the mobilenode.cc which is under /root/NCTUns-6.0/src/nctuns/module/mobile to record the received signal strength value in the hello message)

 

.............................................

#include <mac-802_11.h>

#include <mobile/mobilenode.h>

#include <mbinder.h>

 

//add the following lines

#include <wphyInfo.h>

#include <math.h>

#include <netinet/in.h>

#include <netinet/udp.h>

 

...........................................

int MoblNode::recv(ePacket_ *pkt) {

 

        Packet                     *pkt_, *myp;

        hdr_mac802_11     *mac_hdr;

 

        assert(pkt);

        assert(pkt->DataInfo_);

      

        struct wphyInfo   *pinfo;

 

        struct sent_info

        {

          int nid;

          double x;

          double y;

          double z;

          double str;

          //int recvnid;

        };

        struct sent_info *sh;

        struct ip *myiph;

 

        if(pkt!=NULL && pkt->DataInfo_!=NULL){

          myp=(Packet *)pkt->DataInfo_;

          myiph=(struct ip *)myp->pkt_sget();

          //point to the application header

          sh=(struct sent_info *)((char *)myiph+sizeof(struct ip)+sizeof(struct udphdr));

          if(sh!=NULL)

          {

             pinfo=(struct wphyInfo *)myp->pkt_getinfo("WPHY");

             //write the received signal strength in the received hello message

             sh->str=10*log10(pinfo->RxPr_*1000);

             //printf("MNode nid=%d x=%f y=%f str=%f\n", get_nid(), sh->x, sh->y, sh->str);

          }

        }

      

        pkt_ = (Packet *)pkt->DataInfo_;

        mac_hdr = (struct hdr_mac802_11 *)pkt_->pkt_get();

 

........................................................................................

 

5. Change to /root/NCTUns-6.0/src/nctuns to recompile the simulator.

6

 

[Simulation]

Assumption: Node 1, 2, 3 are all anchor nodes. Node 4 is the unknown node.

Node 1~4 are 802.11 (b) mobile node (ad hoc mode)

1.In the Draw mode, place 4 nodes like the following figure.

7

 

2. Click "E" to change to Edit mode and do some settings.

2.1 Click Node 1 to add the pos_s application running on it from 0 to 10 seconds.

12

 

2.2 Click the Node Editor to change the propagation model to free-space model

9

10

 

2.3 Repeat the 2.1 and 2.2 for Node 2 and Node 3.

 

2.4 Click Node 4 to add the pos_c application running on it from 1 to 10 seconds.

3. Click the "R" to the Run mode and start simulation.

 

4. Click the window that runs "coordinator" to see the simulation results.

From the following figure, we can see ticks=0, node 1, 3, 2 run the pos_s application.

We can also see that the node 1 is at (383,152), node 2 is at (550, 184), and node 3 is at (420, 260).

Then when node 4 (the unknown node) receives three hello messages from node 1, 2, 3, it starts to calculate its node.

In this example, the real position for node 4 is (467, 202). And the calculated position is also (467, 202).

Because all nodes are static and the method we get the distance from the anchor node to itself does not cause error.

(All are based math model calculation. In movement scenario or the case we don't use the model to estimate the distance, estimated error may occur.)

 

13

 

Dr. Chih-Heng Ke (http://csie.nqu.edu.tw/smallko), smallko@gmail.com

Department of Computer Science and Information Engineering, National Quemoy University, Taiwan