matlab練習程序（TOA算法）

TOA全稱time of arrival，根據測量移動台和基站之間的信號到達時間，再轉換為距離從而進行定位的方法。

類似的技術還有TDOA和AOA，真實的應用場景就是GNSS和UWB了。

定位原理可以用下圖表示：

如上圖三個基站可列三個方程，就能解出定位點，但一般基站通常更多，因此需要用到最小二乘的方法解方程組。

設MS坐標為(x，y)，BS坐標為（xi，yi）,則有方程組： 

展開得：

 

 

 

 

 

 

 

 

到這里就能用最小二乘來求解了。 

matlab代碼如下：

clear all;
close all;
clc;

num = 6;                                %可以認為是基站或衛星數，隨機做6個

loc = [20,30]                           %實際坐標位置
sat = 100*rand(num,2);                  %基站或衛星二維坐標

dist = loc - sat + 0.2*rand(num,2);     %加20厘米測距誤差
dist = dist(:,1).^2 + dist(:,2).^2;
r = sat(:,1).^2 + sat(:,2).^2;

X = [-2*sat ones(length(sat),1)];
Y = dist - r;

C = inv(X'*X)*X'*Y;

C(1:2)                                  %計算結果

結果如下：

設定的位置和最終計算得到的位置基本一致。