基于rssi的加权定位算法实现

最近一个项目里需要用到室内定位，以前从没接触过这么"高端"的东东，看了N篇文章终于把这个算法实现了。（PS：以前在学校没觉得查论文有什么，还总不想看，知道学校交的有查论文的钱，但没什么感觉。如今出来工作了看到查个论文还得要钱心里总归是很不爽的）

===========================我是可爱的分割线================================

在理想状态下，即测量误差很小的的情况下，利用AP收到的rssi值可以得到距离d，则，利用三个AP的d，相交于一点（如图1），即移动端的坐标。设三个AP的坐标分别为(x1,y1),(x2,y2),(x3,y3),UE的坐标为(x,y)，UE到AP的距离为d1,d2,d3，则有：

展开后，从第一项开始依次减去最后一项，整理后写成矩阵形式有：AX=b

其中：

根据最小二乘法得：

在b没有误差或误差很小的情况下，这样计算出的坐标值精度比较高的。但是，（以前在学校的时候，英语老师总在说重点在but之后）由于无线信号传播的衰减并不是总符合理论的模型，加上设备本身的误差，造成测量得到的距离误差很大，三个圆并不是会交于一个点，而是交于一个区域或者相离，如图2,3

        图1：三个圆相交于1点            图2：交于一个区域                    图3：相离

出于考虑真实环境下的测量误差，可以做次加权操作，核心思想就是，对于距离近的则精度比较高给予高的权值比重，距离远的则误差比较大，给予低的权值，加权后总和考虑。即：

(x,y) = sum(Wi * (x',y')/sum(Wi)  

Wi = 1 / sum(Ri)

其中，Ri是AP到UE的距离,(x',y')是由Ax=b计算出的结果，(x,y)即为最终结果。在具体计算的时候，先将收到UE的rssi的AP进行分组，有论文指出，AP的数量在4-5个精度比较高，只有3个或者大于5个由于累计误差的原因导致AP数量增多整体误差反而增大！得到C(5，3)个AP的组合，分别计算上式，最后得到(x,y）。

算法流程：

1. 收集各AP收到UE的rssi

2. 按rssi排序，取前4(5)个

3. 计算AP的组合，得到C(5,3)个组合数

4. 遍历每个组合，计算Wi * (x',y')，同时记录totalW

5. 得到(x,y)

==================c#实现=================

需要下载一个mathnet包，python用习惯了，忽然觉得c#好陌生的赶脚

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Reflection;
using DatabaseProvider;
using log4net;
using MathNet.Numerics.LinearAlgebra.Double;


[assembly: log4net.Config.XmlConfigurator(Watch = true)]
namespace Algorithm
{
    /// 

    /// 三角形定位算法实现类
    /// 
    public class CWeightTrilateral
    {
        struct APInfo
        {
            public string uuid;
            public CPoint location;
            public int rssi;
            public int A; //A为距离探测设备1m时的rssi值的绝对值，最佳范围在45-49之间
            public double n; //n为环境衰减因子，需要测试矫正，最佳范围在3.25-4.5之间
            public double height;
        }
               
        static double totalWeight = 0;

        /// 

        /// 根据AP收到的rssi值计算终端UE的位置
        /// 
        /// AP的信息，格式：AP_id:rssi 
        /// 返回终端的坐标
        public static CPoint GetLocation(Dictionary APList)
        {            
            if(APList == null || APList.Count < 3)
            {
                throw new Exception("the number of AP is less then 3, cloud not located the mobel unit, please check AP!");
            }
            
            var apSort = from ap in APList orderby ap.Value descending select ap; //按每个AP收到的rssi值降序排列，取前5个进行计算
            List apArray = new List();            
            foreach(KeyValuePair ap in apSort)
            {               
                APInfo ap_info = GetAPInfo(ap.Key);
                ap_info.uuid = ap.Key;
                ap_info.rssi = ap.Value;                
                apArray.Add(ap_info);
                //取前5个计算，当AP数量多的时候，会造成累计误差增大，严重影响定位精度！
                if (apArray.Count > 5)
                    break;
            }
            //将AP分组
            List