标签:
杂谈 |
分类: localization |
DV-Hop的定位算法,已知网络中几个锚节点就可以计算出网络中其他点的坐标,但这种算法有时由于网络密度的稀疏,可能误差会有一些。
#include<stdio.h>
#include<stdlib.h>
#include<time.h>
#include<math.h>
#include<iostream.h>
#define N 100 // 总节点数
#define R 0.5 // 最大通信距离
#define MAX 4 // 希望的每一个节点上通过的最大路径数
#include<stdlib.h>
#include<time.h>
#include<math.h>
#include<iostream.h>
#define N
#define R
#define MAX
double cordination[200] = // 确定的节点坐标
{
0.219600, -0.389400, -0.712000, -0.419000 , -0.690900, -0.232900, 0.702400, -0.155200, 0.742600, -0.644400,
-0.884500, -0.125000, -0.453200, 0.561600, -0.876600, -0.245000, 0.424800, 0.775800, -0.887000,-0.154400,
0.283600, 0.931100, 0.056000, -0.298900, 0.504900, -0.872400, -0.204100, 0.468400, -0.253000, 0.242200,
-0.821500, 0.780000, -0.895500,0.869400,
0.173800, -0.063200, 0.589000, -0.272600, 0.880000,
-0.176000,
-0.421400, -0.859900, -0.844000, 0.925000, 0.067400, -0.557200, 0.252100, -0.676200, 0.713300, 0.065000,
0.432600, -0.124500, 0.346000, 0.218400, -0.346200,0.948000,
-0.784000, -0.596000, -0.128800, 0.481100,
0.862400, -0.267800, 0.976800, 0.113600, -0.165100,-0.096200,
-0.137600, -0.478400, -0.424200, 0.374000,
-0.856000, 0.695000, -0.285800,-0.104000, 0.021600,
-1.000000, 0.274800, -0.052000, -0.372800, -0.281600,
-0.270000, 0.780600, -0.262500,0.848500, -0.572000,
-0.892800, -0.880000, -0.436800, -0.641400, -0.301900,
-0.270400, 0.288800, -0.712500,0.614800,
0.805600, -0.735000, 0.706300, 0.651000, -0.820400,
-0.856000,
0.601700, 0.147600, -0.064400, 0.432000, 0.797800, 0.549500, 0.388100, 0.454100, 0.555900, -0.382000,
-0.086000, -0.646000, 0.366000,-0.175900, -0.091200,
-0.734000, 0.050600, 0.126000, -0.768400, -0.561600,
-0.785600, -0.015600, 0.174000,-0.888800, 0.058700, 0.480000,
-0.992000, 0.996000, 0.046000, -0.855300,
-0.660000, -0.061600, 0.762500,-0.428800, 0.682000, 0.631700,
-0.120000, 0.784600, -0.153600, -0.806600,
-0.519400, 0.778900, -0.502700,0.059500,
0.113700, 0.616200, 0.563400, 0.129000, -0.886400,
-0.692500,
0.544000, -0.932000, 0.908000, 0.259500, -0.546100,-0.136400,
-0.778700, 0.977100, 0.748800, -0.088400,
-0.986200, -0.619800, 0.053000,-0.766000, -0.082400,
0.339200, 0.566900, -0.159200, -0.631200, -0.034800,
0.768400, 0.515200, 0.697900, -0.156800, -0.841900,-0.045500,
-0.236500, -0.065600, -0.958400, -0.677600,
-0.831400, -0.856500, -0.269100, -0.718000, -0.318000, -0.389500, 0.292700,-0.181600, -0.054400, -0.864000,
0.792400, -0.625600, 0.716600, 0.546000, -0.776000,0.959600,
-0.365800, 0.197200, -0.168000, 0.660000
};
{
0.219600, -0.389400, -0.712000, -0.419000 , -0.690900, -0.232900, 0.702400, -0.155200, 0.742600, -0.644400,
-0.884500, -0.125000, -0.453200, 0.561600, -0.876600, -0.245000, 0.424800, 0.775800, -0.887000,
0.283600, 0.931100, 0.056000, -0.298900, 0.504900, -0.872400, -0.204100, 0.468400, -0.253000, 0.242200,
-0.821500, 0.780000, -0.895500,
-0.421400, -0.859900, -0.844000, 0.925000, 0.067400, -0.557200, 0.252100, -0.676200, 0.713300, 0.065000,
0.432600, -0.124500, 0.346000, 0.218400, -0.346200,
0.862400, -0.267800, 0.976800, 0.113600, -0.165100,
-0.856000, 0.695000, -0.285800,
-0.270000, 0.780600, -0.262500,
-0.270400, 0.288800, -0.712500,
0.601700, 0.147600, -0.064400, 0.432000, 0.797800, 0.549500, 0.388100, 0.454100, 0.555900, -0.382000,
-0.086000, -0.646000, 0.366000,
-0.785600, -0.015600, 0.174000,
-0.660000, -0.061600, 0.762500,
-0.519400, 0.778900, -0.502700,
0.544000, -0.932000, 0.908000, 0.259500, -0.546100,
-0.986200, -0.619800, 0.053000,
0.768400, 0.515200, 0.697900, -0.156800, -0.841900,
-0.831400, -0.856500, -0.269100, -0.718000, -0.318000, -0.389500, 0.292700,
0.792400, -0.625600, 0.716600, 0.546000, -0.776000,
};
struct Point // 等分点结构
{
double x;
double y;
int K; // 等分数
};
{
};
struct
Node
// 节点结构
{
int
id;
// 标号
double position_x; //
横坐标
double position_y; //
纵坐标
char
neb[N];
// 邻接状态表
int
count;
// 路径经过该节点的次数
int
next_id;
// 下一跳节点号
int
hop;
// 到目标节点需要的跳数
double total_distance; //
到目标节点需要传播的距离
double
disoff;
// 下一跳节点偏离等分点的距离
};
{
};
void Init(Node *nod, int *id, int num); //
初始化节点
void Print_Node(Node *nod, FILE *fp);
// 打印初始化的节点信息
void CreateNet(Node *nod, int num);
// 构建节点网络
void Print_Net(Node *nod, FILE *fp);
// 打印某一个节点的网络连接情况
Point Compute_DividePoint(double x1, double y1, double x2, double y2); // 计算等分点坐标
void SearchRoutine(Node *nod, FILE *fp); //
寻找合理路径
void Print_RoutNum(Node *nod, FILE *fp); //
打印节点上通过的路径数
void Compute_repeat(Node *nod, FILE *fp); //
计算重复往返路径数(现在可以保证不会出现重复往返路径)
void Compute_hop_disoff(Node *nod, FILE *fp); //
计算每个节点到目标节点需要经历的跳数和下一跳节点与等分点偏移距离
void Compute_totalhop(Node *nod, FILE *fp); //
计算所有节点到目标节点的总跳数
void Init_new(Node *nod, int *id); //
使用确定的坐标初始化节点
void Compute_totaldisoff(Node *nod, FILE *fp); //
计算所有节点的下一跳节点与相应等分点的偏移距离
void
void
void
Point Compute_DividePoint(double x1, double y1, double x2, double y2);
void
void
void
void
void
void
void
void main()
{
Node nod[N]; // 节点数组
int id =
0;
// 节点id号
FILE *fp1;
FILE *fp2;
FILE *fp3;
FILE *fp4;
char filename1[] = "output.txt";
char filename2[] = "routine.txt";
char filename3[] = "node1.txt";
char filename4[] = "capability.txt";
{
}
double AverageRandom(double min,double max)
{
int minInteger = (int)(min*10000);
int maxInteger = (int)(max*10000);
int randInteger = rand()*rand();
int diffInteger = maxInteger - minInteger;
int resultInteger = randInteger % diffInteger +
minInteger;
return resultInteger/10000.0;
}
{
}
void Init(Node *nod, int *id, int num)
{
int i;
{
}
void Init_new(Node *nod, int *id)
{
int i;
{
}
void Print_Node(Node *nod, FILE *fp)
{
int i;
for(i=0; i<N; i++)
{
fprintf(fp, " %f\t %f \n",
nod[i].position_x,
nod[i].position_y);
fprintf(fp, "\n");
}
}
{
}
void CreateNet(Node *nod, int num)
{
int i;
int j;
double distance;
for(i=0; i<num; i++)
{
for(j=0; j<num; j++)
{
distance =
sqrt(pow((nod[i].position_x-nod[j].position_x), 2)
+
pow((nod[i].position_y-nod[j].position_y), 2)); //
计算两节点间的距离
if(distance <=R)
{
nod[i].neb[j] =
'Y'; // 在通信范围内,'Y'表示有连接
}
else
{
nod[i].neb[j] =
'N'; // 不在通信范围内,'N'表示无连接
}
}
}
}
{
}
void Print_Net(Node *nod, FILE *fp)
{
int i;;
int j;
for(i=0; i<N; i++)
{
fprintf(fp, "在node%d通信范围内的节点 : \n",
nod[i].id);
for(j=0; j<N; j++)
{
if(nod[i].neb[j] != 'N')
{
fprintf(fp, "to node %d :
%c\n", j+1, nod[i].neb[j]);
}
else
{
continue;
}
}
fprintf(fp, "\n");
}
}
{
}
Point Compute_DividePoint(double x1, double y1, double x2,
double y2)
{
Point p;
double distance;
double k1;
int K;
distance = sqrt(pow((x1-x2), 2) + pow((y1-y2),
2)); // 计算两节点间的距离
k1=distance / R;
K = (int)k1 + 1; //
计算等分数
p.x = x1 + (double)(x2-x1)/K;
p.y = y1 + (double)(y2-y1)/K;
return p; // 返回坐标数组
}
{
}
void SearchRoutine(Node *nod, FILE *fp)
{
int i;
int j;
int min;
int flag = 0;
double temp_distan;
int k;
int z;
Point p; // 等分点
double distan[N] ;
double dMin;
for(i=0; i<N;
i++)
{
fprintf(fp, "Node %d to sink :\n",
nod[i].id);
temp_distan = sqrt(pow((nod[i].position_x-0),
2)+pow((nod[i].position_y-0), 2)); // 计算两节点间的距离
if(temp_distan < R) //
可以直接到达目标节点
{
fprintf(fp, "to sink directly.\n");
fprintf(fp, "\n");
nod[i].next_id = 0; //
表示下一跳节点即为目标节点
nod[i].disoff = 0; // 下一跳节点就是目标节点
}
else // 需要经历中继节点
{
p = Compute_DividePoint(nod[i].position_x,
nod[i].position_y, 0, 0);
temp_distan = sqrt(pow((nod[i].position_x-p.x),
2)+pow((nod[i].position_y-p.y), 2));
{
}
void Print_RoutNum(Node *nod, FILE *fp)
{
int i;
int totalsum = 0;
fprintf(fp, "\n节点经历的路径数: \n");
for(i=0; i<N; i++)
{
fprintf(fp, "node %d : %d
\n", nod[i].id, nod[i].count);
totalsum += nod[i].count;
}
fprintf(fp, "\n");
fprintf(fp, "总路径数: %d", totalsum);
}
{
}
void Compute_repeat(Node *nod, FILE *fp)
{
int count = 0;
int i;
for(i=0; i<N; i++)
{
if(nod[i].next_id ==
nod[nod[i].next_id-1].next_id)
{
count++;
}
}
fprintf(fp, "\n重复往返路径数: %d\n", count);
}
{
}
void Compute_hop_disoff(Node *nod, FILE *fp)
{
int i;
int flag;
fprintf(fp,
"\n各节点到目标节点的跳数及下一跳等分点偏移:\n");
for(i=0; i<N; i++)
{
flag = nod[i].next_id;
nod[i].hop = 1;
while(flag != 0)
{
flag = nod[flag-1].next_id;
nod[i].hop++;
if(nod[i].hop == 50) //
假定若跳数达到50时还未到目标节点,则与目标节点肯定无可能路径
{
nod[i].hop = 999; // 置为一个很大的数
break;
// 防止死循环
}
}
if(nod[i].hop == 999)
{
fprintf(fp, "节点 %d: 没有可能路径!;
下一跳等分点偏移 %f\n", nod[i].id, nod[i].disoff);
}
else
{
fprintf(fp, "节点 %d: 跳数 %d; 下一跳等分点偏移
%f\n",
nod[i].id,
nod[i].hop, nod[i].disoff);
}
}
}
{
}
void Compute_totalhop(Node *nod, FILE *fp)
{
int i;
int totalhop = 0;
for(i=0; i<N; i++)
{
if(nod[i].hop != 999)
{
totalhop += nod[i].hop;
}
}
fprintf(fp, "\n总跳数: %d\n", totalhop);
}
{
}
void Compute_totaldisoff(Node *nod, FILE *fp)
{
int i;
double totaldisoff = 0;
for(i=0; i<N; i++)
{
totaldisoff += nod[i].disoff;
}
fprintf(fp, "总偏移距离: %f\n",
totaldisoff);
}
{
}
后一篇:UKF MATLAB 实现