室内定位系统

最近学校实验要做一个室内定位系统，由于本人计算机无法运行Android虚拟机，而且也不支持外接Android设备（就是这么强），所以自己参照WLAN的API和相关操作制作了一个基于三角定位的程序。A，c参数不通用，要根据实际WiFi信号自己测量，然后用MATLAB进行非线性拟合求出（其实精度要求不大的时候自己算也行）。本程序可以在VS2010上跑通，在7.8m*9.0m的房间内误差为0.7-0.8m左右，有待改进。希望能给需要的人带来帮助，抛砖引玉也未尝不可。

#ifndef UNICODE

#define UNICODE
#endif


#include <windows.h>
#include <wlanapi.h>
#include <objbase.h>
#include <wtypes.h>
#include <math.h>


#include <stdio.h>
#include <stdlib.h>


#pragma comment(lib, "wlanapi.lib")
#pragma comment(lib, "ole32.lib")


typedef struct point 
{
double x;
double y;
double z;
}point,vec;//表示点的坐标或者空间向量


//通过信号强度计算距离
double distance(int rssi)
{
double A=/*418.5276*/103.3688,c=/*-21.11187*/-1.888;
int rssii = abs(rssi);
float power = (rssii - /*59*/A)/(10*/*2.0*/c);
return pow(10,power);
}


//三角定位算法
point  triangulation(point A,point B,point C,double sa,double sb,double sc)//求点P的坐标
{
double ab = sqrt(pow(A.x - B.x,2)+pow(A.y - B.y,2));
double ac = sqrt(pow(A.x - C.x,2)+pow(A.y - C.y,2));
double bc = sqrt(pow(B.x - C.x,2)+pow(B.y - C.y,2));


double a = sa,b = ab,c = ac,d = bc,e = sc,f = sb;


//空间海伦-秦九韶公式
double D = b*b + c*c - d*d;
double E = a*a + c*c - e*e;
double F = a*a + b*b - f*f;

double V = sqrt(4*a*a*b*b*c*c - a*a*D*D - b*b*E*E - c*c*F*F + D*E*F)/12;


//平面海伦-秦九韶公式
double p = (ac + ab + bc)/2;
double S = sqrt(p*(p-a)*(p-b)*(p-c));


double h = V*3/S;


double d1 = sqrt(sa*sa - h*h),d2 = sqrt(sb*sb - h*h), d3 = sqrt(sc*sc - h*h);


double A1 = 2*(B.x - A.x),B1 = 2*(B.y - A.y),C1 = d1*d1 - d2*d2 - A.x*A.x + B.x*B.x - A.y*A.y + B.y*B.y;
double A2 = 2*(C.x - B.x),B2 = 2*(C.y - B.y),C2 = d2*d2 - d3*d3 - B.x*B.x + C.x*C.x - B.y*B.y + C.y*C.y;


point P;
P.x = (B2*C1 - B1*C2)/(A1*B2 - A2*B1);
if(P.x<0.000001) P.x = 0;
P.y = (C1*A2 - C2*A1)/(B1*A2 - B2*A1);
if(P.y<0.000001) P.y = 0;
P.z = A.z;


return P;
}




int wmain()
{
    // 声明，初始化变量


/*printf("distance:%f\n",distance(95));
printf("distance:%f\n",distance(92));
printf("distance:%f\n",distance(85));
printf("distance:%f\n",distance(82));
printf("distance:%f\n",distance(84));
printf("distance:%f\n",distance(80));
printf("distance:%f\n",distance(66));
*/
HANDLE hClient = NULL;
    DWORD dwMaxClient = 2;      //    
    DWORD dwCurVersion = 0;
    DWORD dwResult = 0;
    DWORD dwRetVal = 0;
    int iRet = 0;




    
    WCHAR GuidString[39] = {0};
WCHAR name0[] = {'r','u','l','e','r','0'};
WCHAR name1[] = {'r','u','l','e','r','1'};
WCHAR name2[] = {'r','u','l','e','r','2'};
WCHAR temp[10];
DOUBLE sa = 0,sb = 0,sc = 0;
    unsigned int i, j, k;


    /* 用于WLAN枚举界面的变量 */


    PWLAN_INTERFACE_INFO_LIST pIfList = NULL;
    PWLAN_INTERFACE_INFO pIfInfo = NULL;


    PWLAN_AVAILABLE_NETWORK_LIST pBssList = NULL;
    PWLAN_AVAILABLE_NETWORK pBssEntry = NULL;
    
    int iRSSI = 0;


    dwResult = WlanOpenHandle(dwMaxClient, NULL, &dwCurVersion, &hClient);
    if (dwResult != ERROR_SUCCESS) {
        wprintf(L"WlanOpenHandle failed with error: %u\n", dwResult);
        return 1;
    }


    dwResult = WlanEnumInterfaces(hClient, NULL, &pIfList);
    if (dwResult != ERROR_SUCCESS) {
        wprintf(L"WlanEnumInterfaces failed with error: %u\n", dwResult);
        return 1;
    } else {
        for (i = 0; i < (int) pIfList->dwNumberOfItems; i++) {
            pIfInfo = (WLAN_INTERFACE_INFO *) &pIfList->InterfaceInfo[i];
           
            iRet = StringFromGUID2(pIfInfo->InterfaceGuid, (LPOLESTR) &GuidString, 
                sizeof(GuidString)/sizeof(*GuidString)); 
            dwResult = WlanGetAvailableNetworkList(hClient,
                                             &pIfInfo->InterfaceGuid,
                                             0, 
                                             NULL, 
                                             &pBssList);


            if (dwResult != ERROR_SUCCESS) {
                wprintf(L"WlanGetAvailableNetworkList failed with error: %u\n",
                        dwResult);
                dwRetVal = 1;
            } else {
                wprintf(L"界面中可用的无线局域网如下:\n");


                wprintf(L"  Num Entries: %lu\n\n", pBssList->dwNumberOfItems);


                for (j = 0; j < pBssList->dwNumberOfItems; j++) {
                    pBssEntry =
                        (WLAN_AVAILABLE_NETWORK *) & pBssList->Network[j];


                    wprintf(L"  Profile Name[%u]:  %ws\n", j, pBssEntry->strProfileName);
                    
                    wprintf(L"  SSID[%u]:\t\t ", j);
                    if (pBssEntry->dot11Ssid.uSSIDLength == 0)
                        wprintf(L"\n");
                    else {   
                        for (k = 0; k < pBssEntry->dot11Ssid.uSSIDLength; k++) {
                            wprintf(L"%c", (int) pBssEntry->dot11Ssid.ucSSID[k]);
temp[k] = pBssEntry->dot11Ssid.ucSSID[k];

                        }




                        wprintf(L"\n");
                    }
                        
                    wprintf(L"  BSS Network type[%u]:\t ", j);
                    switch (pBssEntry->dot11BssType) {
                    case dot11_BSS_type_infrastructure   :
                        wprintf(L"Infrastructure (%u)\n", pBssEntry->dot11BssType);
                        break;
                    case dot11_BSS_type_independent:
                        wprintf(L"Infrastructure (%u)\n", pBssEntry->dot11BssType);
                        break;
                    default:
                        wprintf(L"Other (%lu)\n", pBssEntry->dot11BssType);
                        break;
                    }
                                
                    printf("  BSSID 数量:\t %u\n", j, pBssEntry->uNumberOfBssids);


                    wprintf(L"  是否可连接:\t ", j);
                    if (pBssEntry->bNetworkConnectable)
                        wprintf(L"是\n");
                    else {
                        wprintf(L"否\n");
                        wprintf(L"  不可连接的原因:\t %u\n", 
                            pBssEntry->wlanNotConnectableReason);
                    }        


                    wprintf(L"  Number of PHY types supported[%u]:\t %u\n", j, pBssEntry->uNumberOfPhyTypes);


                    if (pBssEntry->wlanSignalQuality == 0)
                        iRSSI = -100;
                    else if (pBssEntry->wlanSignalQuality == 100)   
                        iRSSI = -50;
                    else
                        iRSSI = -100 + (pBssEntry->wlanSignalQuality/2);    
                        
                    printf(" 信号强度(RSSI): %i (dBm)\n 距离(distance): %f(M)",  
                        iRSSI,distance(iRSSI));
                    wprintf(L"\n");
                    
                    wprintf(L"\n");


if(!strcmp((const char*)temp,(const char*)name0))
sa = distance(iRSSI);
else if(!strcmp((const char*)temp,(const char*)name1))
sb = distance(iRSSI);
else if(!strcmp((const char*)temp,(const char*)name2))
sc = distance(iRSSI);
else
{


}


                }
            }
        }


    }


//wprintf(L"sa:%f,sb:%f,sc:%f",sa,sb,sc);


point a,b,c;
a.x = 0;a.y = 0 ;a.z = 2.2;
b.x = 7.8;b.y = 0 ;b.z = 2.2;
c.x = 0;c.y = 9.0;c.z = 2.2;  
point P = triangulation(a,b,c,sa,sb,sc);
printf("目标所在坐标为：\n X:%.4f Y:%.4f",P.x,P.y);




//释放空间
    if (pBssList != NULL) {
        WlanFreeMemory(pBssList);
        pBssList = NULL;
    }


    if (pIfList != NULL) {
        WlanFreeMemory(pIfList);
        pIfList = NULL;
    }
system("pause");
    return dwRetVal;
}