clear

clc.

%Establish coordinate system

for i=1:1: 10

for j=1:1: 10

x (j+(i-1)*10)=(i-1)*10;

y (j+(i-1)*10)=(j-1) * 10;

end

end

figure

plot(x,y,'.')

hold on

%Randomly distributed 50 nodes to be located within the coordinate system, indicated by a red asterisk

axis([0 100 0 100])

xy=[x;y]

hold on

xm=90;

ym=90;

n=50.;

for i=1:1: n

Sx(i)=rand(1,1)*xm;

Sy(i)=rand(1,1)*ym;

plot(Sx(i), Sy(i), ' r*')

xlabel('x axis')

ylabel ('Y axis')

hold on

end

dm=30;

m=100.;

for j=1:1: n

SS=[Sx(j);Sy(j)];

k=0.;

DM as the radius, look for each node to be positioned around the anchor node

for i=1:1:m

d=norm((xy (:, i) - SS), 2);

%%%%%%%%%%% Set weights%%%%%%%%%%%%

if d< = dm

%xy is a 2-row m-column matrix, all abscissa is the first row, and the ordinate is the second row; xx (j, i）

%Represents the abscissa of the i-th anchor node of the j-th node to be located

p(j, i)=1/d ;

xx (j, i)=xy(1, i);

yy(j,i)=xy(2, i);

k=k+1;

else.

p(j, i)=0;

xx(j, i)=0;

yy(j, i)=0;

end

end
The average coordinates of all anchor nodes within the range of calculation%, that is, the centroid,
expressed as a hollow circle

if k~=0

cent (:, j)=[sum(xx(j,:));sum (yy(j,:))]/k;

else.

cent (:, j)=0;

end

plot (cent(1, j), cent (2, j), 'o')

hold on

% Distance weighted improved centroid, expressed in triangles

if k~=0

centx (:, j)=[sum (xx(j,:).*p(j,:));sum (yy(j,:).*p(j,:))]/(sum(p (j,:)));

else.

centx (:, j)=0;

end

plot (centx(1, j), centx(2, j),'^')

hold on

%Draw each node to be located away from the centroid belongs to a green line

plot ([cent (1, j) Sx (j)], [cent (2, j) Sy(j)], 'g')

Title ('Centroid')

hold on

%Draw each node to be located away from the weighted centroid belongs to a red line

plot ([centx(1, j) Sx (j)], [centx(2, j) Sy(j)], 'm')

Title ('xCentroid')

hold on

%Find each node to be positioned positioning error

MM=[cent (1, j);cent (2, j)]

e (j)=norm ((MM-SS), 2)/dm

NN=[centx(1, j); centx(2, j)]

err(j)=norm((NN-SS), 2)/dm

end

figure

axis([0 n 0 10])

j=1:1: n

The %green line indicates the error between the center of mass and the actual position

plot (j, e(j), ' - G.')

hold on

%Red line indicates the error between the weighted centroid and the actual position

plot (j, err(j), ' - m.')

hold on

Title ('Centroid')

E=sum (e) / n

ERR=sum (err)/n

clear all;
close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%% Values %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

PT=-15.28;
GT=2.2;
GR=1;%1.5
L=0;
x=0:0.125:10;
y=0:0.125:10;
lambda=(3*10^8)/(2.4*10^9);
barx=0;
bary=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%% Access Points %%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Access point coordinates

xap1=0; %2
yap1=0; %4
xap2=3.5; %5
yap2=9; %6
xap3=7; %8
yap3=0; %8

for i=1:1000
PA1(i)=-69.72+sqrt(0.3^2)*randn(1,1);
PA2(i)=-64.68+sqrt(0.5^2)*randn(1,1);
PA3(i)=-65.63+sqrt(0.8^2)*randn(1,1);
end

for p=1:40

% First Access Point

for i=1:1:81
for j=1:1:81
a=sqrt((xap1-x(i)).^2+(yap1-y(j)).^2)-0.5;
PR1(i,j)=PT+GT+GR-L+20*log10(lambda/(4*pi))-20*log10(a);
end;
end;

%PA1=-69.72; %4/7
F1=(PR1-PA1(p)).^2;
FU1=1./(1+F1);

% Second Access Point

for i=1:1:81
for j=1:1:81
a=sqrt((xap2-x(i)).^2+(yap2-y(j)).^2)-1;
PR2(i,j)=PT+GT+GR-L+20*log10(lambda/(4*pi))-20*log10(a);
end;
end;

%PA2=-64.68; %4/7
F2=(PR2-PA2(p)).^2;
FU2=1./(1+F2);

% Third Access Point

for i=1:1:81
for j=1:1:81
a=sqrt((xap3-x(i)).^2+(yap3-y(j)).^2)-1;
PR3(i,j)=PT+GT+GR-L+20*log10(lambda/(4*pi))-20*log10(a);
 end;
end;

%PA3=-65.63; %4/7
F3=(PR3-PA3(p)).^2;
FU3=1./(1+F3);

% All Access Points

for i=1:1:81
for j=1:1:81
FU4(i,j)=FU1(i,j)*FU2(i,j)*FU3(i,j);
end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%% Maximum %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

position=[1,1];
m=0;
a=1;
b=1;

for i=1:5
max(1,i)=0;
max(4,i)=0;
end;

for i=1:1:81
for j=1:1:81
for g=1:1:5
if FU4(i,j)>max(1,g)
ma=max(1,g);
max(1,g)=FU4(i,j);
for h=5-g+1:1:5
ma=max(1,h);
max(1,h)=ma;
end;
max(2,g)=i/8;
max(3,g)=j/8;
break;
end;
end;
end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% Plots %%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%subplot(2,2,1), surf(x,y,FU1),xlabel('x'); ylabel('y');zlabel('position

information'),title('First Access Point');
%subplot(2,2,2), surf(x,y,FU2),xlabel('x'); ylabel('y');zlabel('position
information'),title('Second Access Point');
%subplot(2,2,3), surf(x,y,FU3),xlabel('x'); ylabel('y');zlabel('position
information'),title('Third Access Point');
 for i=1:1:5
barx=max(2,i)+barx;
bary=max(3,i)+bary;
end;

max;
barx=barx/5;
bary=bary/5;

erreurcase2=((bary-4)^2+(barx-7)^2)^(0.5);
sauvegardeerreur(p)=erreurcase2;
%subplot(2,2,4), surf(x,y,FU4),xlabel('x'); ylabel('y');zlabel('position
information'),title(erreurcase2);

sauvx(p,1)=barx;
sauvy(p,1)=bary;
variancex=cov(sauvx);
variancey=cov(sauvy);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%% Kalman Filter %%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initial State
%%%%%%%%%%%%%%%
if p==1
initialx=0;
initialy=0;
X=[initialx;initialy];
end
sigmax=10^(-5);
sigmay=10^(-5);
B=10;
pb=[B 0;0 B];

% Matrix definition
%%%%%%%%%%%%%%%%%%%
A=[1 0;0 1];
H=[1 0;0 1];
R=[variancex 0;0 variancey];

% Kalman Filter Iteration

%%%%%%%%%%%%%%%%%%%%%%%%%
% Time update -> Predict

xb=A*X;
pb=A*pb*A'+[sigmax 0;0 sigmay];

% Measurement update -> Correct

K=pb*H*inv(H*pb*H'+R);
X=xb+K*(H*[barx;bary]-H*xb); % use of the triangulation measure
sauv(p,1)=X(1,1);
sauv(p,2)=X(2,1);
pb=(eye(2)-K*H)*pb;

end

%'Nouvelle serie'
%X
t=1:40;
subplot(2,1,1); plot(t,sauv(:,1)),hold,
plot(t,mean(sauvx(:,1)).*ones(1,40),'r'),title('evolution of the x axis measure')
subplot(2,1,2); plot(t,sauv(:,2)),hold,
plot(t,mean(sauvy(:,1)).*ones(1,40),'r'),title('evolution of the y axis measure')

clear all, clc;

for n=6:2: 30

x=100*rand(1,100); %10m*10m grid area

y=100*rand(1,100);

w=100*rand(1, n);

z=100*rand(1, n);

plot(x, y, ' b*',w, z, 'rO')

axis([0 100 0 100])

grid on;

xlabel ('x'),ylabel ('y')

title ('original point distribution')

C=0;

X=zeros(1,100);

Y=zeros(1,100);

for i=1: 100

m=0; a=0; b=0;

for k=1: n

dist = distance (x(i), y(i), w(k), z(k)); distance between %node and anchor node

if dist<=10% communication radius value=2

a=a+w(k);
 b=b+z(k);

m=m+1;

end

end

if m>=2

X(i)=a/m;

Y(i)=b/m;

%plot ([x(i),X(i)],[y(i), Y(i)]);

%hold on;

else X (i)=0;

Y(i)=0;

C=C+1% number of points not available

end

end

plot(x, y, ' b*',w, z, 'rO',X,Y, 'bO')

axis([0 100 0 100])

grid on;

xlabel ('x'),ylabel ('y')

title ('point distribution after positioning')

ALE=0;

for i=1: 100

% if X(i)~=0&Y(i)~=0
 ALE=ALE+sqrt((X(i)-x(i))^2+(Y(i)-y(i))^2);

% end

end

Ale=ALE/100; %positioning error value

ALE=ALE/4;

c1(n/2-2)=(100-C)/100;

ale1(n/2-2)=ALE;

bili(n/2-2)=n/(100+n); %anchor node ratio

end

figure ;

plot(bili,c1);

grid on;

xlabel ('anchor node scale'), ylabel ('anchor node scale')

title ('anchor node ratio and the proportion of nodes can be located in Fig.');

figure,

plot(bili, ale1);

xlabel ('anchor node ratio'), ylabel ('positioning error')

grid on;

title ('anchor node scale and positioning error')

% converts received signal strength to distance

The % transmit signal is attenuated to the receiving end, and the TR distance is
calculated according to the strength of the received signal.

% Receive power Pr should be measured by actual measurement

% but in the absence of experimental equipment, it is also possible to derive
simulated measurements using assumed unknown nodes
The % method is: according to the assumed unknown node position, each beacon node gets
accurate receiving power.
% Add a Gaussian random variable as the environmental interference, and use this
received power as the measured value of Pr.
% then use the measured value of Pr as the RSSI to find the TR distance

% divides the region into a number of adjacent triangles, placing the beacon nodes at
the vertices of the triangle
% can also be said to be the sensor nodes randomly but as evenly distributed as
possible in the area, after being positioned as a beacon node
% unknown nodes transmit positioning signals to the surroundings, and each beacon node
receives its distance from the unknown node by using RSSI ranging algorithm after
receiving.
% selects the three smallest distances from these distances and uses its corresponding
beacon node as the selected beacon node.
% to ensure that the unknown node is inside the triangle formed by the selected beacon
node
% TR distance in the following program refers to the distance between the selected
beacon node and the unknown node

Function [r] = Distance(d,a)

PtW = 10e3; % unit is W

Pt = 10*log10(PtW); % unit is dB
f = 9e8; % carrier frequency in Hz
n = 2; % path loss index
D0 = 20; % near ground reference distance in m
%d = 100*sqrt(13) % The exact TR distance between the selected beacon node and the
unknown node, in m
c = 3*10^8; % speed of light in m/s
Lamida = c/f; % wavelength in m
Gt = 1; Gr = 1; L = 1; %Gt is the transmit antenna gain; Gr is the receive antenna
gain; L is the system loss factor independent of propagation (not less than 1)

%PL0 is the path loss of the near ground reference distance

%PrW = PtW*Gt*Gr*lamida^2/((4*pi)^2*d0^2*L) % unit is W
%PL0 = 10*log10(Pt/Pr) % unit is dB
PL0 = -10*log10(Gt*Gr*lamida^2/((4*pi)^2*d0^2*L)); % unit is dB
Pr0 = Pt-PL0; % unit is dB

%PL is the path loss of the precise TR distance; Pr is the received power of the
beacon node
PL = PL0+10*n*log10(d/d0); % unit is dB
Pr = Pt - PL; % unit is dB
PrW = 10^(Pr/10); % unit is W

%RSSI is the received signal strength indication, here is the received power
containing Gaussian random variables
%Xn is a zero-mean Gaussian distribution random variable with a standard deviation
of cigema
%PrG is the received power of the Gaussian random variable, which is used to
simulate the measured value of the received power in dB.
Cigema = 11.8; % unit is dB
N = 5e3;
Xn = normrnd(0, cigema, N, 1);
X = mean(Xn); % mean
PrG = Pr+X; % unit is dB
 PrGW = 10^(PrG/10); % unit is W
RSSI = PrGW; % unit is W

%r is the calculated TR distance; a is the parameter, which varies with distance

range
%RSSI = a*(1/r)^2
%a = 7; % is within the distance of the selected beacon node. After repeated
testing, this parameter is more suitable.
r = 1/sqrt(RSSI/a);

clear all

v=150; % % target speed

v_sensor=0;%%sensor speed

t=1; %%scan cycle

xradarpositon=0; %%sensor coordinates

yradarpositon=0; %%

ppred=zeros(4,4);

Pzz=zeros(2,2);

Pxx=zeros(4,2);

xpred=zeros(4,1);

ypred=zeros(2,1);

sumx=0;

sumy=0;

sumxukf=0;

sumyukf=0;

sumxekf=0;
sumyekf=0; %%%of the initial value of the statistics

L=4;

alpha = 1;

kalpha=0;

belta=2;

ramda=3-L;

azimutherror=0.015; % % mean square error

rangeerror=100; % % distance mean square error

processnoise=1; % % process noise mean square error

tao=[t^3/3 t^2/2 0 0;

t^2/2 t 0 0;

0 0 t^3/3 t^2/2;

0 0 t^2/2 t]; % % the input matrix of process

G=[t^2/2 0

t0

0 t^2/2

0 t ];

a=35*pi/180;

a_v=5/100;

a_sensor=45*pi/180;
x (1)=8000; %%initial position

y(1)=12000;

for i=1: 200

x(i+1)=x(i)+v*cos(a)*t;

y(i+1)=y(i)+v*sin(a)*t;

end

for i=1: 200

xradarpositon=0;

yradarpositon=0;

Zmeasure(1, i)=atan((y(i)-yradarpositon)/(x (i)-xradarpositon))+random ('Normal',0,azimutherror, 1, 1);

Zmeasure(2, i)=sqrt ((y (i)-yradarpositon)^2+(x(i)-xradarpositon)^2) + random('Normal', 0, rangeerror, 1,

1);

xx (i)=Zmeasure(2, i)*cos (Zmeasure(1, i)); % % observations

yy(i)=Zmeasure(2, i)*sin (Zmeasure(1, i));

measureerror=[azimutherror^2 0; 0 rangeerror^2];

processerror=tao*processnoise;

vNoise = size(processerror, 1);

wNoise = size (measureerror,1);

A=[1 t 0 0;
0 1 0 0;

0 0 1 t;

0 0 0 1];

Anoise=size(A,1);

for j=1:2*L+1

Wm (j)=1/(2*(L+ramda));

Wc (j)=1/(2*(L+ramda));

end

Wm(1)=ramda/(L+ramda);

Wc (1)=ramda/(L+ramda);%+1-alpha^2+belta; % % weight

if i= = 1

xerror = rangeerror^2*cos (Zmeasure(1, i))^2+Zmeasure(2, i)^2*azimutherror^2*sin(Zmeasure (1,i))^2;

yerror = rangeerror^2*sin(Zmeasure (1,i))^2+Zmeasure(2, i)^2*azimutherror^2*cos(Zmeasure (1,i))^2;

xyerror=(rangeerror^2-Zmeasure(2, i)^2*azimutherror^2) * sin (Zmeasure(1, i))*cos (Zmeasure(1, i));

P=[xerror xerror/t xyerror xyerror/t;

xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t ^ 2);

xyerror xyerror/t yerror yerror/t;

xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t ^ 2)];

xestimate=[Zmeasure(2, i)*cos (Zmeasure(1, i)) 0 Zmeasure(2, i)*sin (Zmeasure(1, i)) 0 ]';

end

cho=(chol(P*(L + ramda)))';%
for j=1:L

xgamaP1 (:, j)=xestimate+cho (:, j);

xgamaP2 (:, j)=xestimate-cho (:, j);

end

Xsigma=[xestimate xgamaP1 xgamaP2];

F=A;

Xsigmapre=F*Xsigma;

xpred=zeros(Anoise,1);

for j=1:2*L+1

xpred=xpred+Wm (j)*Xsigmapre (:, j);

end

Noise1=Anoise;

ppred=zeros(Noise1, Noise1);

for j=1:2*L+1

ppred=ppred+Wc (j)*(Xsigmapre (:, j) - xpred)*(Xsigmapre (:, j) - xpred)';

end

ppred=ppred+processerror;

chor=(chol((L+ramda)*ppred))';

for j=1:L

XaugsigmaP1 (:, j)=xpred+chor (:, j);

XaugsigmaP2 (:, j)=xpred-chor (:, j);

end

Xaugsigma=[xpred XaugsigmaP1 XaugsigmaP2 ];

for j=1:2*L+1

Ysigmapre(1, j)=atan(Xaugsigma(3, j)/Xaugsigma(1, j)) ;

Ysigmapre(2, j)=sqrt((Xaugsigma(1, j))^2+(Xaugsigma(3, j))^2);

end

ypred=zeros(2,1);

for j=1:2*L+1

ypred=ypred+Wm (j)*Ysigmapre (:, j);

end

Pzz=zeros(2,2);

for j=1:2*L+1

Pzz=Pzz+Wc(j)*(Ysigmapre (:, j) - ypred)*(Ysigmapre (:, j) - ypred)';

end

Pzz=Pzz+measureerror;

Pxy=zeros(Anoise, 2);

for j=1:2*L+1

Pxy=Pxy+Wc(j)*(Xaugsigma (:, j) - xpred)*(Ysigmapre (:, j) - ypred)';

end

K=Pxy*inv(Pzz);

xestimate=xpred+K*(Zmeasure (:, i)-ypred);

P=ppred-K*Pzz*K';

xukf(i)=xestimate(1,1);

yukf(i)=xestimate(3,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%% EKF PRO%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if i= = 1

ekf_p=[xerror xerror/t xyerror xyerror/t;

xerror/t 2*xerror/(t^2) xyerror/t 2*xyerror/(t ^ 2);

xyerror xyerror/t yerror yerror/t;

xyerror/t 2*xyerror/(t^2) yerror/t 2*yerror/(t ^ 2)];

ekf_xestimate=[Zmeasure(2, i)*cos (Zmeasure(1, i)) 0 Zmeasure (2, i)*sin (Zmeasure(1, i)) 0 ]';

ekf_xpred=ekf_xestimate;

end;

F=A;

ekf_xpred=F*ekf_xestimate;

ekf_ppred=F*ekf_p*F'+processerror;

H=[-ekf_xpred(3)/(ekf_xpred (3)^2+ekf_xpred (1)^2) 0 ekf_xpred(1)/(ekf_xpred(3)^2+ekf_xpred (1)^2) 0;

ekf_xpred(1)/sqrt(ekf_xpred(3)^2+ekf_xpred (1)^2) 0 ekf_xpred (3)/sqrt(ekf_xpred (3)^2+ekf_xpred

(1)^2) 0];

ekf_z(1,1)=atan(ekf_xpred(3)/ekf_xpred(1)) ;

ekf_z(2,1)=sqrt ((ekf_xpred(1))^2+(ekf_xpred(3))^2);

PHHP=H*ekf_ppred*H'+measureerror;

ekf_K=ekf_ppred*H' * inv(PHHP);

ekf_p=(eye (L)-ekf_K*H)*ekf_ppred;
ekf_xestimate=ekf_xpred+ekf_K*(Zmeasure (:, i)-ekf_z);

traceekf(i)=trace(ekf_p);

xekf(i)=ekf_xestimate(1,1);

yekf(i)=ekf_xestimate(3,1);

errorx (i)=xx (i)+xradarpositon-x(i);

errory (i)=yy (i)+yradarpositon-y(i);

ukferrorx (i)=xestimate(1)+xradarpositon-x(i);

ukferrory(i)=xestimate (3)+yradarpositon-y(i);

ekferrorx(i)=ekf_xestimate(1)+xradarpositon-x(i);

ekferrory(i)=ekf_xestimate(3)+yradarpositon-y(i);

aa (i)=xx (i)+xradarpositon-x(i);;

bb (i)=yy (i)+yradarpositon-y(i);

sumx=sumx+(errorx (i)^2);

sumy=sumy+(errory (i)^2);

sumxukf=sumxukf+(ukferrorx(i)^2);

sumyukf=sumyukf+(ukferrory(i)^2);

sumxekf=sumxekf+(ekferrorx(i)^2);

sumyekf=sumyekf+(ekferrory(i)^2);

mseerrorx(i)=sqrt(sumx/(i-1)); statistical mean square error of%noise

mseerrory(i)=sqrt(sumy/(i-1));
mseerrorxukf(i)=sqrt(sumxukf/(i-1)); statistical mean square error of%UKF

mseerroryukf(i)=sqrt(sumyukf/(i-1));

mseerrorxekf(i)=sqrt(sumxekf/(i-1)); statistical mean square error of%EKF

mseerroryekf(i)=sqrt(sumyekf/(i-1));

end

figure(1);

plot(mseerrorxukf, 'r');

hold on;

plot(mseerrorxekf, 'g');

hold on;

plot (mseerrorx,'.');

hold on;

ylabel('MSE of X axis', 'fontsize',15);

xlabel('sample number', 'fontsize',15);

legend ('UKF', 'EKF', 'measurement error');

figure(2)

plot(mseerroryukf, 'r');

hold on;

plot(mseerroryekf, 'g');

hold on;

plot (mseerrory,'.');
hold on;

ylabel('MSE of Y axis', 'fontsize',15);

xlabel('sample number', 'fontsize',15);

legend ('UKF', 'EKF', 'measurement error');

figure(3)

plot(x, y);

hold on;

plot(xekf, yekf, 'g');

hold on;

plot (xukf, yukf, 'r');

hold on;

plot(xx,yy, 'm');

ylabel(' X', 'fontsize',15);

xlabel ('Y', 'fontsize',15);

legend ('TRUE', 'UKF', 'EKF', 'measurements');