在博文到達時間差測量目標位置(tdoa)定位一文中,我們推導了chan演算法的數學原理。chan演算法可將tdoa中的求解多個雙曲線的非線性方程求解,化為帶引數的偽線性方程,可大大降低運算量。但在求解引數r
0r_0
r0時,會存在無解,乙個解,兩個解三種情況。即chan演算法,存在不可定位和定位模糊的可能,在出現定位模糊時,需要我們引入新的時間差資訊,對其中的乙個偽解進行剔除。
**圖如下:
下面給出chan演算法的matlab實現:
clear all; close all; clc;
stations = [0 0 0;-20 20 30;20 20 -40;20 10 -20;20 30 40]; %基站位置
c = 3e8; %光速
x = -100:100; %生成目標位置
y = 3*x.*sin(x);
n = length(x);
locations = zeros(n,3);
tds = zeros(n,length(stations)-1);
for i = 1:n
tds(i,:) = td(stations*1e3,[x(i),y(i),0]*1e3,c);
endfor i = 1:n
locations(i,:) = tdoa1(stations*1e3,tds(i,:),c)/1e3;
endfigure(1);
plot(x,y,'r*');
hold on;
plot(locations(:,1),locations(:,2),'b--o');
legend('真實位置','定位位置');
xlabel('km');
ylabel('km');
%% 根據目標位置,生成觀測樣本(時差資訊)
function [tds] = td(stations,t,c)
n = length(stations)-1;
tds = zeros(1,n);
rs = zeros(1,n);
r0 = sqrt((t(1)-stations(1,1))^2+(t(2)-stations(1,2))^2+(t(3)-stations(1,3))^2);
for i = 1:n
rs(i) = sqrt((t(1)-stations(i+1,1))^2+(t(2)-stations(i+1,2))^2+(t(3)-stations(i+1,3))^2);
tds(1,i) = (rs(i)-r0)/c;
endend
%% 根據時差,基站位置反推目標位置,即chan演算法的核心實現
function [location] = tdoa1(stations,tds,c)
n = length(stations)-1;
%%%%%%%%%%%%%%%
a = zeros(n-1,3);
for i = 1:n
a(i,1) = stations(i+1,1)-stations(1,1);
a(i,2) = stations(i+1,2)-stations(1,2);
a(i,3) = stations(i+1,3)-stations(1,3);
end%%%%%%%%%%%%%%%%%%%
b1 = (-tds*c)'; %[n-1,1],delta r
d1 = zeros(n,1);
for i = 1:n
d1(i,1) = stations(i+1,1)^2+ stations(i+1,2)^2+ stations(i+1,3)^2;
endl = (1/2)*(d1-(stations(1,1)^2+ stations(1,2)^2+ stations(1,2)^2)-(b1.^2));
a2 = a(1:3,:);
l2 = l(1:3,:);
b2 = b1(1:3,:);
a = det(a2);
a1 = (det([b2,a2(:,2:3)]))/a;
a2 = (det([a2(:,1),b2,a2(:,3)]))/a;
a3 = (det([a2(:,1:2),b2]))/a;
b1 = (det([l2,a2(:,2:3)]))/a;
b2 = (det([a2(:,1),l2,a2(:,3)]))/a;
b3 = (det([a2(:,1:2),l2]))/a;
d = a1^2+a2^2+a3^2-1; %a
c1 = (b1-stations(1,1));
c2 = (b2-stations(1,2));
c3 = (b3-stations(1,3));
e = a1*c1+a2*c2+a3*c3; %b
f =c1^2+c2^2+c3^2; %c
g = sqrt(e^2-d*f);
r01 = (-e+g)/d;
r02 = (-e-g)/d;
location1 = [a1*r01+b1;a2*r01+b2;a3*r01+b3];
location2 = [a1*r02+b1;a2*r02+b2;a3*r02+b3];
td***1 = td(stations,location1,c);
td***2 = td(stations,location2,c);
if(abs(td***1(4)-tds(4))<1e-8) % 引入時差資訊,剔除偽解
location = location1;
else
location = location2;
endend
綜合演算法 不考慮誤差的TDOA定位
tdoa是一種無線定位技術,是一種利用時間差進行定位的方法。不同於toa,tdoa 到達時間差 是通過檢測訊號到達兩個基站的時間差,而不是到達的絕對時間來確定移動臺的位置,降低了時間同步要求。tdoa至少需要三個已知座標位置的基站,通過獲取不同基站之間的訊號傳送時間差來定位。假設三個基站座標分別為 ...
分治思想求解X的M次冪方
1 package main23 import 4 fmt 5 67 遞迴形式分治求解 8 func power x,m int int else 17returny18 19 20 21 迭代形式分治求解,分析可用到如下圖 x的任意m次方,可從x的一次方,開始向上迭代產生,而路徑的選擇則根據m的二...
小黑演算法成長日記6 最大連續m子段和求解
arr 2,11,4,13 5,6,2 def maxsum arr,m n len arr if m n print 輸入引數不合法 return b n 1 0 for i in range m 1 max 0for i in range 1 m 1 對角線長度方向遍歷 for j in ran...