基于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
{
    /// <summary>
    /// 三角形定位算法实现类
    /// </summary>
    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;

        /// <summary>
        /// 根据AP收到的rssi值计算终端UE的位置
        /// </summary>
        /// <param name="APList">AP的信息，格式：AP_id:rssi </param>
        /// <returns>返回终端的坐标</returns>
        public static CPoint GetLocation(Dictionary<string,int> 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<APInfo> apArray = new List<APInfo>();            
            foreach(KeyValuePair<string,int> 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<object> temp = apArray.ConvertAll(s => (object)s);
            CCombineAlgorithm ca = new CCombineAlgorithm(temp.ToArray(), 3);
            object[][] combineAPArray = ca.getResult();
            CPoint deviceLocation = new CPoint();
            for (int i = 0; i < combineAPArray.GetLength(0); i++)
            {                
                List<APInfo> apList = new List<APInfo>();
                foreach(object obj in combineAPArray[i])
                {
                    apList.Add((APInfo)obj);
                }
                //得到加权后的坐标
                deviceLocation += CaculateByAPList(apList);
            }

            return new CPoint(deviceLocation.X / totalWeight, deviceLocation.Y / totalWeight);
        }

        /// <summary>
        /// 根据三角形定位算法计算UE位置
        /// </summary>
        /// <param name="ap_list"></param>
        /// <returns>返回UE的定位坐标(x,y)</returns>
        private static CPoint CaculateByAPList(List<APInfo> apArray)
        {
            double[,] a_array = new double[2, 2];
            double[] b_array = new double[2];

            //距离数组
            double[] distanceArray = new double[3];
            for(int i = 0; i< 3; i++)
            {
                distanceArray[i] = GetDisFromRSSI(apArray[i]);
            }

            //系数矩阵A初始化
            for (int i = 0; i< 2; i++)
            {
                a_array[i, 0] = 2 * (apArray[i].location.X - apArray[2].location.X);
                a_array[i, 1] = 2 * (apArray[i].location.Y - apArray[2].location.Y);
            }
 
            //矩阵b初始化
            for (int i = 0; i< 2; i++)
            {
                b_array[i] = Math.Pow(apArray[i].location.X, 2)
                            - Math.Pow(apArray[2].location.X, 2)
                            + Math.Pow(apArray[i].location.Y, 2)
                            - Math.Pow(apArray[2].location.Y, 2)
                            + Math.Pow(distanceArray[2], 2)
                            - Math.Pow(distanceArray[i], 2);
            }
            var matrixA = DenseMatrix.OfArray(a_array);
            var vectorB = new DenseVector(b_array);
            //计算 X=(A^T * A)^-1 * A^T * b
            var a1 = matrixA.Transpose(); // A的转置
            var a2 = a1 * matrixA;
            var resultX = a2.Inverse() * a1 * vectorB;
            double[] res = resultX.ToArray();                      

            /*对应的权值*/
            double weight = 0;
            for (int i = 0; i < 3; i++)
            {
                weight += (1.0 / distanceArray[i]);
            }
            totalWeight += weight;

            return new CPoint(res[0] * weight, res[1] * weight);
        }

        /// <summary>
        /// 根据AP的ID在数据库中查找配置信息
        /// </summary>
        /// <param name="ap_id"></param>
        /// <returns>返回AP的配置信息</returns>
        private static APInfo GetAPInfo(string ap_uuid)
        {
            ILog log = log4net.LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
            CAPInfoProvider apPro = new CAPInfoProvider();
            AP_Info_table entity = apPro.GetEntityByUUID(ap_uuid);
            APInfo apInfo = new APInfo();
            try
            {
                apInfo.location = new CPoint(entity.x.Value, entity.y.Value);
                apInfo.A = entity.A.Value;
                apInfo.n = entity.n.Value;
                apInfo.height = entity.height.Value;                    
            }
            catch(Exception ex)
            {
                log.Error(string.Format("get AP entity  from {0} error! please check AP UUID!", ap_uuid));
            }            
            return apInfo;
        }

        /// <summary>
        /// 利用RSSI得到设备距AP的水平距离
        /// </summary>
        /// <param name="ap_info">参与运算的AP实例</param>        
        /// <returns>单位：m</returns>
        private static double GetDisFromRSSI(APInfo ap_info)
        {
            double rawDis = 0.0;           
            double power = (ap_info.A - ap_info.rssi) / (10 * ap_info.n);
            rawDis = Math.Pow(10, power);
            //返回AP到UE的水平距离
            return Math.Sqrt(Math.Pow(rawDis, 2) - Math.Pow(ap_info.height, 2));           
        }
    }
}