功能: 坐标定位

格式:
SLAM_LoactionByDist(tagPoints, dist, weight)
SLAM_LoactionByDist(tagPoints, dist)

tagPoints: 一个列数为3的矩阵变量
dist     : 元素个数与tagPoints行数一致的矩阵变量
weight   : 元素个数与tagPoints行数一致的矩阵变量, 此参数默认情况下为全1的矩阵变量

说明:
1. 本函数主要计算这么一类问题: 
(1) 已知3D空间中多个tag的坐标(这些坐标分别对应tagPoints的每一行数值)
(2) 空间一未知点p, 距离各个tag的距离为dist(即dist每个元素值对应距离)
(3) 点p与各个tag距离的可信度用weight来表示
(4) 现在想知道p点的3D坐标信息.

2. 本函数返回的是1*3的矩阵信息.

3. 本函数本质上是空间中3D定位.

4. 注意, 本函数计算的仅是3D空间坐标, 并没有朝向信息.

例子:
//现在已知空间中有5个点,每个点坐标存储在如下tag, 空间中一未知点p距离tag的距离为dist, 每个距离可信度为w, 求p点位置
tag =
[ 23.7082060071212    66.2390687811370    60.8763112038729
  75.9823441393591    41.2517252570259    3.42611130486527
  51.1805663589298    98.2255275818638    99.7680061961375
  29.4545861563900    61.8398258284851    22.7692779725274
  33.3568346841991    2.03172876594203    0.69610439273347 ]

dist =
[ 90.2924458092062
  79.0020367443962
  145.569273719543
  68.0963975592683
  25.4997667856251 ]

w =
[ 0.86000000000000
  1.50000000000000
  1.00000000000000
  0.30000000000000
  0.65000000000000 ]

p = SLAM_LoactionByDist(tag, dist, w)//执行命令,得到p点位置
p =
[ 7.87267997378371    1.35941563556638    0.10960504313823 ]