定位算法

1、核心算法

zigbee发来的盲节点到参考节点的距离值，然后发送给上位机，在QT编写的上位机上对盲节点的位置进行计算。

下面是zigbee代码里面的RSSI转距离算法：

double rssitoDistance(uint8 rssi,int a, double n)
{
      double ra = fabs(rssi);
      double ka = (ra - a) / (10 * n);
      double distance = pow(10, ka);
      return distance;
}

下面是QT接收到盲节点到各个参考节点的距离，之后的都是QT上位机处理函数，算法见最下面：

//三边定位
          if(th->temp[0]=='L')
          {
               if(th->temp[1]=='1')
               {
                    location1=(th->temp[2]-48)*1+(th->temp[3]-48)*0.1+(th->temp[4]-48)*0.01;
                      qDebug() << location1<<endl;
               }
               if(th->temp[8]=='3')
               {
                    location2=(th->temp[9]-48)*1+(th->temp[10]-48)*0.1+(th->temp[11]-48)*0.01;
                      qDebug() << location2<<endl;
               }
               if(th->temp[15]=='4')
               {
                    location3=(th->temp[16]-48)*1+(th->temp[17]-48)*0.1+(th->temp[18]-48)*0.01;
                   qDebug() << location3<<endl;
               }
               getLocation(location1,location2,location3);//处理函数

          }

//三边定位
void MainWindow::getLocation(double x, double y, double z)
{
    typedef double type;
    int i,j,n;
    n=3;
    type **p=zeros(n,3);
    p[0][0]=0.0;
    p[0][1]=0.0;
    p[0][2]=x;
    p[1][0]=0.0;
    p[1][1]=5.0;
    p[1][2]=y;
    p[2][0]=5.0;
    p[2][1]=0;
    p[2][2]=z;

    type **p1=zeros(n-1,2);  //A矩阵
    for(i=0;i<n-1;i++)
    {
        for(j=0;j<2;j++)
        {
            p1[i][j]=2*(p[i][j]-p[n-1][j]);
        }
    }
    type **q1=zeros(n-1,1);     //b矩阵
    for(i=0;i<n-1;i++)
    {
        for(j=0;j<1;j++)
        {
        //	q1[i][j]=p[i][0]*p[i][0]+p[i][1]*p[i][1]-p[i][2]*p[i][2]-p[n-1][0]*p[n-1][0]-p[n-1][1]*p[n-1][1]+p[n-1][2]*p[n-1][2];
            q1[i][j]=pow(p[i][0],2)+pow(p[i][1],2)-pow(p[i][2],2)-pow(p[n-1][0],2)-pow(p[n-1][1],2)+pow(p[n-1][2],2);
        }
    }
    type **p2=zeros(2,n-1); //矩阵A的装置
    p2=MatrixConver(p1, n-1, 2);
    type **p3=zeros(2,2);
    p3=MatrixMulti(p2,p1,2,n-1,2); //矩阵AT*A
    type **p4=zeros(2,2);     //矩阵AT*A求逆矩阵
    p4=MatrixInver(p3,2);
    type **p5=zeros(2,n-1);    //(AT*A)-1*AT
    p5=MatrixMulti(p4,p2,2,2,n-1);
    type **p6=zeros(2,1);      //x矩阵
    p6=MatrixMulti(p5,q1,2,n-1,1);
   // printf("x=%3.2f  y=%3.2f\n",p6[0][0],p6[1][0]);
   // qDebug()<< p6[0][0]<<endl;
   // qDebug()<< p6[1][0]<<endl;

    locationWidget->getxy(p6[0][0],p6[1][0]);

}

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
double **zeros(int m,int n)    //动态开辟m行n列的double型元素数组
{
    int i,j;
    double **p;
    p=(double **)malloc(m*sizeof(double *));    //开辟一个指针数组返回为指针的指针
    for(i=0;i<m;i++)
    {
        p[i]=(double *)malloc(n*sizeof(double));          //每个指针元素指向一个包含n个double型元素的数组首地址
    }
    for(i=0;i<m;i++)
    {
        for(j=0;j<n;j++)
        {
            p[i][j]=0;
        }
    }
    return p;
}
double **MatrixConver(double **a, int m, int n)
{
    int i, j;
    double **p=zeros(n,m);
    for(i=0;i<n;i++)
    {
        for(j=0;j<m;j++)
        {
            p[i][j]=a[j][i];
        }

    }
    return p;
}
double **MatrixMulti(double **a,double **b,int ra,int ab,int cb)
{
    int i,j,k;
    double **p=zeros(ra,cb);
    for(i=0;i<ra;i++)
    {
        for(j=0;j<cb;j++)
        {
            for(k=0;k<ab;k++)
            {
                p[i][j]+=a[i][k]*b[k][j];
            }
        }
    }
    return p;

}
double **MatrixInver(double **a,int N)
{
    double t;
    double **b,**c;
    int i,j,m;
    b=zeros(N,2*N);
    c=zeros(N,N);

    /*增广矩阵（A|E）存入二维数组b中*/
    for(i=0;i<N;i++)
        for(j=0;j<N;j++)
            b[i][j]=a[i][j];
        for(i=0;i<N;i++)
            b[i][N+i]=1;
        for(m=0;m<N;m++)       //对每行进行处理。
        {
            t=b[m][m];      //预存b[m][m]。
            i=m;
            while(b[m][m]==0)
            {
                b[m][m]=b[i+1][m];
                i++;
            }
            if(i>m) //将第b[m][m]=0的行与b[i][m]！=0的行交换
            {
                b[i][m]=t;    //交换其它各列相应位置的元素
                for(j=0;j<m;j++)
                {
                    t=b[m][j];
                    b[m][j]=b[i][j];
                    b[i][j]=t;
                }
                for(j=m+1;j<2*N;j++)
                {
                    t=b[m][j];
                    b[m][j]=b[i][j];
                    b[i][j]=t;
                }
            }
            for(i=m+1;i<N;i++)
            {
                for(j=2*N-1;j>=m;j--)  //m=0时，将第一行的-b[i][0]/b[0][0]倍加到以下各行。这样以下每行第一个元素b[i][0]就为。
                    b[i][j]=b[i][j]-b[i][m]*b[m][j]/b[m][m];
            }
            for(j=2*N-1;j>=m;j--)
                b[m][j]=b[m][j]/b[m][m];   //对第m行作行变换，同除以b[m][m]，使b[m][m]为。
        }
        m=N-1;
        while(m>0)
        {
            for(i=0;i<m;i++)
                for(j=2*N-1;j>=m;j--)//千万注意，此处j必须递减，否则b[i][m]先变为，后面的计算就无效！
                    b[i][j]-=b[i][m]*b[m][j];
                m--;
        }
        for(i=0;i<N;i++)  //将逆矩阵存入二维数组c中。
            for(j=0;j<N;j++)
                c[i][j]=b[i][N+j];
    return c;
}
double distance(double *p,double *q)
{
    double s;
    s=sqrt(pow((p[0]-q[0]),2)+pow((p[1]-q[1]),2))+2*rand()/(RAND_MAX+1.0);//加入噪声污染
    return s;
}

2、民间开发套件