作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

个人主页：Matlab科研工作室

个人信条：格物致知。

更多Matlab仿真内容点击

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

内容介绍

由于目标信号的发射时间未知,无源定位技术大多利用TDOA(到达时间差)进行目标定位.本文将求解GPS单点定位的Bancroft算法

完整代码

function [eX,eb] = algebraicGPSequations(satPos, tVals)

% Calculates the position of a receiver given the time delay of arrival to

% at least 4 satellites in 3D (or at least 3 satellites in 2D).

% INPUTS: satPos: is Cartesian location of satellites (in 1D, 2D, or 3D).

%               Each row is the position of one satellite.

%         tVals: is the arrival time of the signal = distance from

%         satellite to reciever + offset b.

% OUTPUTS: eX: estimated position of the user.

%          eb: estimate of the offset time b.

% 

% Implements the solution of Stephen Bancroft, "An Algebraic Solution of the GPS

% Equations," in IEEE Transactions on Aerospace and Electronic Systems,

% vol. AES-21, no. 1, pp. 56-59, Jan. 1985, doi: 10.1109/TAES.1985.310538.

% https://ieeexplore.ieee.org/document/4104017

% 

% pdf at

% https://cas.tudelft.nl/Education/courses/ee4c03/assignments/indoor_loc/Bancroft85loc.pdf

%  Aaron T. Becker, University of Houston

%  Version 1.0.2:  Feb 23, 2021 initial submission

%  Version 1.0.3:  Feb 05, 2023 updated by adjusting the documentation

%% Initialization

if nargin < 1

    disp('No arguments provided, so this generates an example problem.')

    % Default values: The node (satellite) positions are randomized. The transmitter

    % is randomly positioned outside of the area covered by the nodes.

    nSat = 4; % number of nodes (satellites)

    dim = 3;  %TODO: fails with dim = 1.  Not sure why.  Works for 2 and 3 dimensions

    satPos = 50*rand(nSat,dim)-25;  % position of nodes

    if dim == 2

        th = 2*pi*rand; %random angle

        Xpos = (50+(50*rand)).*[cos(th) sin(th)];%user location (unknown to satellites)

    else

        Xpos = 150*rand(1,dim)-75;

    end

    b = 15+15*rand;  %  timing offset  (unknown to satellites)

    noiseLvl = 0; % Ideal signal + Noise.

    tVals = sqrt(sum((Xpos-satPos).^2,2))+b + noiseLvl*rand(nSat,1);  % Eqn (2)

    fprintf("Running algebraicGPSequations with nSat = %d, in %dD.\n Randomly generated satellite positions.\n Actual user position is:",nSat,dim );

    disp(Xpos)

else

    nSat = length(tVals);

    b = NaN;  % unknown to satellites

    Xpos = NaN(1,size(satPos,2)); % unknown to satellites

end

if nSat < size(satPos,2) +1

    disp(['Error: system is underdetermined with ',num2str(nSat),' satellites and ', num2str(size(satPos,2)), ' dimensions'])

end

%% calculation of user location and time offset

A = [satPos, tVals];                         % Eqn (5)

i0 = ones(nSat,1);                           % Eqn (6)

r = ones(nSat,1);                            % Eqn (7)

for i = 1:nSat

    r(i) = minkowskiSum(A(i,:),A(i,:))/2;   % Eqn (8)

end

B = pinv(A);  % same as inv(A'*A)*A';       % Eqn (9)

u = B*i0;                                   % Eqn (10)

v = B*r;                                    % Eqn (11)

E = minkowskiSum(u,u);                      % Eqn (12)

F = minkowskiSum(u,v) - 1;                  % Eqn (13)

G = minkowskiSum(v,v);                      % Eqn (14)

lam1 = (-2*F+sqrt((2*F)^2-4*E*G))/(2*E);    % Eqn (15) +solution to quadratic

lam2 = (-2*F-sqrt((2*F)^2-4*E*G))/(2*E);    % Eqn (15) -solution to quadratic

y1 = lam1*u+v;                              % Eqn (16) +

y2 = lam2*u+v;                              % Eqn (16) -

Tx1 = y1(1:end-1)';                         % Eqn (17)

Tx2 = y2(1:end-1)';                         % Eqn (17)

b1 = -y1(end);                              % Eqn (17)

b2 = -y2(end);                              % Eqn (17)

%determine which position answer is right by calculating the average error

tvals1 = sqrt(sum((Tx1-satPos).^2,2))+b1;

tvals2 = sqrt(sum((Tx2-satPos).^2,2))+b2;

% e1 = mean(abs(tvals1-tVals));

% e2 = mean(abs(tvals2-tVals));

e1 = std(tvals1-tVals);

e2 = std(tvals2-tVals);

if e1 < e2

    legent = {'Satellite positions','X position','estimate 1 (correct)','estimate 2'};

    eX = Tx1;

    eb = b1;

else

    legent = {'Satellite positions','X position','estimate 1','estimate 2 (correct)'};

    eX = Tx2;

    eb = b2;

end

%% Plot the process

cplot = @(r,x0,y0,linet) plot(x0 + r*cos(linspace(0,2*pi)),y0 + r*sin(linspace(0,2*pi)),linet);

if size(satPos,2) == 1

    figure(1); clf; hold on;

    p(1) = plot(satPos,zeros(size(satPos)),'bx');

    hold on

    for i = 1:length(satPos)

        cplot(tVals(i)-b,satPos(i),0,'g-');

    end

    p(2) = plot(Xpos(1),0, 'xg');

    set(p(2),'color', [0,0.5,0])

    xlabel('x');ylabel('y')

    axis equal

    if  abs(max(tVals)-min(tVals) - (max(satPos)-min(satPos))) < 1e-10

        [minV,minInd] = min(satPos);

        [maxV,maxInd] = max(satPos);

        legent = {'Satellite positions','X position'};

        if tVals(maxInd) < tVals(minInd)

            str = sprintf("The receiver position is unknown, but > %.2f",maxV);

            a = axis;

            reg = patch([max(satPos),max(satPos),a(2),a(2)],[a(4),a(3),a(3),a(4)],[0.5,0.5,1]);

            uistack(reg,'bottom')

        else

            str = sprintf("The receiver position is unknown, but < %.2f",minV);

            a = axis;

            reg = patch([min(satPos),min(satPos),a(1),a(1)],[a(4),a(3),a(3),a(4)],[0.5,0.5,1]);

            uistack(reg,'bottom')

        end

        title(sprintf('Act X = [%.2f], b=%.2f \n %s',Xpos(1),b,str))

    else

        p(3) = plot(Tx1(1),0, 'ob');

        p(4) = plot(Tx2(1),0, 'or'); 

        for i = 1:length(satPos)

            cplot(tVals(i)-b,satPos(i),0,'g-');

            cplot(tVals(i)-b1,satPos(i),0,'b--');

            cplot(tVals(i)-b2,satPos(i),0,'r--');

        end

        title(sprintf('Act X = [%.2f], b=%.2f \n Est X = [%.2f], b=%.2f',Xpos(1),b,eX(1),eb))

    end

    legend(p,legent);

    axis tight

    disp(['Act X = [',num2str(Xpos),'], b=',num2str(b)])

    disp(['Est X = [',num2str(eX),  '], b=',num2str(eb)])

    disp('std(error),   b,       Tx  for two solutions:')

    disp([e1,b1,Tx1])

    disp([e2,b2,Tx2])

elseif size(satPos,2) == 2

    figure(1); clf; hold on;

    p(1) = plot(satPos(:,1),satPos(:,2),'bx');

    hold on

    for i = 1:length(satPos)

        cplot(tVals(i)-b,satPos(i,1),satPos(i,2),'g-');

        cplot(tVals(i)-b1,satPos(i,1),satPos(i,2),'b--');

        cplot(tVals(i)-b2,satPos(i,1),satPos(i,2),'r--');

    end

    

    p(2) = plot(Xpos(1),Xpos(2), 'x');

    set(p(2),'color', [0,0.5,0])

    p(3) = plot(Tx1(1),Tx1(2), 'ob');

    p(4) = plot(Tx2(1),Tx2(2), 'or');

    

    legend(p,legent);

    xlabel('x');ylabel('y')

  

    title(sprintf('Act X = [%.2f,%.2f], b=%.2f \n Est X = [%.2f,%.2f], b=%.2f',Xpos(1),Xpos(2),b,eX(1),eX(2),eb))

    axis equal

elseif size(satPos,2) == 3

    figure(1); clf; hold on;

    p(1) = plot3(satPos(:,1),satPos(:,2),satPos(:,3),'bx');

    hold on

    [X,Y,Z] = sphere;

    lightGrey = 0.8*[1 1 1];

    for i = 1:length(satPos)

        radius = tVals(i)-b;

        surf(satPos(i,1)+radius*X,satPos(i,2)+radius*Y,satPos(i,3)+radius*Z,'facecolor','g','facealpha',0.02,'EdgeColor',lightGrey,'edgealpha',0.2)

    end

    

    p(2) = plot3(Xpos(1),Xpos(2),Xpos(3), 'x');

    set(p(2),'color', [0,0.5,0])

    p(3) = plot3(Tx1(1),Tx1(2),Tx1(3), 'ob');

    p(4) = plot3(Tx2(1),Tx2(2),Tx2(3), 'or');

    

    legend(p,legent);

    xlabel('x');ylabel('y');zlabel('z')

  

    title(sprintf('Act X = [%.2f,%.2f,%.2f], b=%.2f \n Est X = [%.2f,%.2f,%.2f], b=%.2f',Xpos(1),Xpos(2),Xpos(3),b,eX(1),eX(2),eX(3),eb))

    axis equal

    view([1 1 0.75]) % adjust the viewing angle

elseif size(satPos,2) > 2

    disp(['plot is only supported for 1D & 2D, this is ',num2str(size(satPos,2)),'D'])

    disp(['Act X = [',num2str(Xpos),'], b=',num2str(b)])

    disp(['Est X = [',num2str(eX),  '], b=',num2str(eb)])

    disp('std(error),   b,       Tx  for two solutions:')

    disp([e1,b1,Tx1])

    disp([e2,b2,Tx2])

end

    function mSum = minkowskiSum(a,b)

        mSum = sum(a(1:end-1).*b(1:end-1)) - a(end)*b(end);  % Eqn (4)

    end

end

运行结果

参考文献

[1]林云松, 孙卓振, and 彭良福. "基于Bancroft算法的多点定位TOA-LS估计." 电子学报 40.003(2012):621-624.

[2]林云松, 孙卓振, 彭良福. 基于Bancroft算法的多点定位TOA-LS估计[J]. 电子学报, 2012(03):207-210.​

️ 完整代码

️部分理论引用网络文献，若有侵权联系博主删除

️ 关注我领取海量matlab电子书和数学建模资料