Recently, a project needs to use indoor positioning. I have never been in contact with such a "high-end" stuff before. After reading N articles, I finally realized this algorithm. (PS: In the past, I didn’t feel that there was anything in checking papers at school, and I still didn’t want to read them. I knew that the school paid for checking papers, but I didn’t feel it. Now that I go out to work and see that a paper has to be paid for, I’m always very upset. 
) _
============================ I am the cute dividing line================== ===============
In an ideal state, that is, when the measurement error is small, the distance d can be obtained by using the rssi value received by the AP. Then, using the d of the three APs, they intersect at a point (as shown in Figure 1), that is, the coordinates of the mobile terminal. Suppose the coordinates of the three APs are (x1, y1), (x2, y2), (x3, y3), the coordinates of the UE are (x, y), and the distance from the UE to the AP is d1, d2, d3, then there are :
After expansion, subtract the last item in turn from the first item, and write it in matrix form after sorting: AX=b
in:
According to the least squares method:
In the case that b has no error or a small error, the coordinate value calculated in this way has a relatively high precision. However, (when I was in school, the English teacher always said that the focus was after the but 
) because the attenuation of wireless signal propagation does not always conform to the theoretical model, plus the error of the device itself, resulting in a large error in the measured distance, three The circles do not meet at a point, but at an area or separate, as shown in Figures 2 and 3
Figure 1: Three circles intersect at 1 point Figure 2: Intersection at an area Figure 3: Separation
In order to consider the measurement error in the real environment, a sub-weighting operation can be performed. The core idea is to give a higher weight to the closer distance, and to give a higher weight to the distance, and to give a lower weight to the far distance. After the sum is considered. which is:
(x,y) = sum(Wi * (x',y')/sum(Wi)
Wi = 1 / sum(Ri)
Among them, Ri is the distance from the AP to the UE, (x', y') is the result calculated by Ax=b, and (x, y) is the final result. In the specific calculation, the APs that receive the UE's rssi are firstly grouped. Some papers point out that the number of APs is relatively high in the range of 4-5, and only 3 or more than 5 are due to the cumulative error. The number of APs increases. The overall error has increased instead! A combination of C(5, 3) APs is obtained, and the above formula is calculated respectively, and finally (x, y) is obtained.
Algorithm flow:
1. Collect the rssi received by each AP from the UE
2. Sort by rssi, take the first 4 (5)
3. Calculate the combination of AP and get C(5,3) combinations
4. Traverse each combination, calculate Wi * (x',y'), and record totalW at the same time
5. get (x,y)
=================== c# implementation ===================
I need to download a mathnet package, python is used to it, and suddenly I feel that c# is so unfamiliar
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>
/// Triangle positioning algorithm implementation class
/// </summary>
public class CWeightTrilateral
{
struct APInfo
{
public string uuid;
public CPoint location;
public int rssi;
public int A; //A is the absolute value of the rssi value at a distance of 1m from the detection device, and the optimal range is between 45-49
public double n; //n is the environmental attenuation factor, which needs to be tested and corrected, and the optimal range is between 3.25-4.5
public double height;
}
static double totalWeight = 0;
/// <summary>
/// Calculate the location of the terminal UE according to the rssi value received by the AP
/// </summary>
/// <param name="APList">AP information, format: AP_id:rssi </param>
/// <returns>Returns the coordinates of the terminal</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; // Arrange in descending order according to the rssi value received by each AP, take the first 5 for calculation
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);
// Take the first 5 calculations. When the number of APs is large, the cumulative error will increase, which will seriously affect the positioning accuracy!
if (apArray.Count > 5)
break;
}
// group APs
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);
}
// get the weighted coordinates
deviceLocation += CaculateByAPList(apList);
}
return new CPoint(deviceLocation.X / totalWeight, deviceLocation.Y / totalWeight);
}
/// <summary>
/// Calculate the UE position according to the triangle positioning algorithm
/// </summary>
/// <param name="ap_list"></param>
/// <returns>Returns the UE's positioning coordinates (x,y)</returns>
private static CPoint CaculateByAPList(List<APInfo> apArray)
{
double[,] a_array = new double[2, 2];
double[] b_array = new double[2];
// distance array
double[] distanceArray = new double[3];
for(int i = 0; i< 3; i++)
{
distanceArray[i] = GetDisFromRSSI(apArray[i]);
}
//Initialize coefficient matrix 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);
}
//initialize matrix 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);
//Calculate X=(A^T * A)^-1 * A^T * b
var a1 = matrixA.Transpose(); // transpose of A
var a2 = a1 * matrixA;
var resultX = a2.Inverse() * a1 * vectorB;
double[] res = resultX.ToArray();
/*corresponding weight*/
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>
/// Find the configuration information in the database according to the ID of the AP
/// </summary>
/// <param name="ap_id"></param>
/// <returns>Return AP configuration information</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>
/// Use RSSI to get the horizontal distance between the device and the AP
/// </summary>
/// <param name="ap_info">AP instance participating in the operation</param>
/// <returns>Unit: 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);
//Return the horizontal distance from AP to UE
return Math.Sqrt(Math.Pow(rawDis, 2) - Math.Pow(ap_info.height, 2));
}
}
}