%Determine the number of satellites, satellite properties and estimated location
ast=input('Enter the latitude of station \n');
bst=input('Enter the longitude of station \n');
ap=input('Enter the latitude of point \n');
bp=input('Enter the longitude of point \n');
n=input('Enter the number of satellites \n');
for i=1:n
fprintf('Enter the height of the satellite %i in meters \n',i)
rs(i)=input('');
fprintf('Enter the alpha of the satellite %i in degrees \n',i)
as(i)=input('');
as(i)=(as(i)*2*pi)/360;
fprintf('Enter the beta of the satellite %i in degrees \n',i)
bs(i)=input('');
bs(i)=(bs(i)*2*pi)/360;
fprintf('Enter the distance between the satellite %i and the station, send from satellite %i \n',i,i)
ds(i)=input('');
fprintf('Enter the distance between the satellite %i and the point where we take measurement,send from satellite %i \n',i,i)
dp(i)=input('');
end
%Radius of earth and sigma for normal distribution
re=6371000;
s=10;
%Refining the values of dp(i) and converting them to d(i) to put them in algortihm
%First finding the error ratio and applying them to dp(i) and converting them to d(i)
ast=(ast*2*pi)/360;
bst=(bst*2*pi)/360;
for i=1:n
des(i)=(re^2+rs(i)^2-2*re*rs(i)*(cos(bst)*cos(ast)*cos(bs(i))*cos(as(i))+cos(bst)*sin(ast)*cos(bs(i))*sin(as(i))+sin(bst)*sin(bs(i))))^(1/2);
err(i)=ds(i)/des(i);
d(i)=dp(i)/err(i);
end
%Normally we won't use ap and bp we will find alpha and between for each satellite circle pairs but it is time consuming
ap=(ap*2*pi)/360;
bp=(bp*2*pi)/360;
%Defining the square ambiguity area,normally found from maximum and minumum alpha and betas
%that found from intersection points of circles, but now we use alpha and beta of point
%Using s value we choose, we calculated the most possible interval for probability calculation
for i=1:3
a=ap-0.00002:0.000001:ap+0.00002;
b=bp-0.00002:0.000001:bp+0.00002;
sizea=length(a);
sizeb=length(b);
pab=ones(sizea,sizeb);
%Finding the probabilities for alpha,beta pairs
for k=1:sizea
for l=1:sizeb
for i=1:n
dab(i)=(re^2+rs(i)^2-2*re*rs(i)*(cos(b(l))*cos(a(k))*cos(bs(i))*cos(as(i))+cos(b(l))*sin(a(k))*cos(bs(i))*sin(as(i))+sin(b(k))*sin(bs(i))))^(1/2);
pdab(i)=normpdf(dab(i),d(i),s);
if i==n
for j=1:n
pab(k,l)=pdab(j)*pab(k,l);
end
end
end
end
end
%Plot of the probability distribution of alpha's and beta's
figure,meshc(a,b,pab),xlabel('alpha'),ylabel('beta'),zlabel('Probability of points')
%To find the centroid of the volume
sumofmass=0;
sumofmoma=0;
sumofmomb=0;
for k=1:sizea
for l=1:sizeb
sumofmass=pab(k,l)+sumofmass;
sumofmoma=pab(k,l)*a(k)+sumofmoma;
end
end
for l=1:sizeb
for k=1:sizea
sumofmomb=pab(k,l)*b(l)+sumofmomb;
end
end
centera=sumofmoma/sumofmass;
centerb=sumofmomb/sumofmass;
%To find the maximum probable point
mostprobablepoint=max(max(pab));
for k=1:sizea
for l=1:sizeb
if pab(k,l)==mostprobablepoint
mostprobablealpha=a(k);
mostprobablebeta=b(l);
end
end
end
%Revising our initial values to refine the calculations
ap=centera;
bp=centerb;
for i=1:n
d(i)=(re^2+rs(i)^2-2*re*rs(i)*(cos(bp)*cos(ap)*cos(bs(i))*cos(as(i))+cos(bp)*sin(ap)*cos(bs(i))*sin(as(i))+sin(bp)*sin(bs(i))))^(1/2);
end
end
%Conversion to degree-minute-second from radian
%for estimation of point using the center of mass of probability
lat=centera*360/(2*pi);
long=centerb*360/(2*pi);
if long<0
hemilong=('South');
else
hemilong=('North');
end
if lat<0
hemilat=('West');
else
hemilat=('East');
end
degreelong=fix(long);
minutelong=fix((long-degreelong)*60);
secondlong=(long-degreelong-minutelong/60)*60;
degreelat=fix(lat);
minutelat=fix((lat-degreelat)*60);
secondlat=(lat-degreelat-minutelat/60)*60;
%Output of the system for mass center
fprintf('\n Your position estimation using mass center\n')
fprintf('%i %i %d %s \n',degreelong,minutelong,secondlong,hemilong)
fprintf('%i %i %d %s \n',degreelat,minutelat,secondlat,hemilat)
%Conversion to degree-minute-second from radian
%for estimation of point using most probable point
latp=mostprobablealpha*360/(2*pi);
longp=mostprobablebeta*360/(2*pi);
if longp<0
hemilongp=('South');
else
hemilongp=('North');
end
if latp<0
hemilatp=('West');
else
hemilatp=('East');
end
degreelongp=fix(longp);
minutelongp=fix((longp-degreelongp)*60);
secondlongp=(longp-degreelongp-minutelongp/60)*60;
degreelatp=fix(latp);
minutelatp=fix((latp-degreelatp)*60);
secondlatp=(latp-degreelatp-minutelatp/60)*60;
%Output of the system for most probable point
fprintf('\n Your position estimation using most probable point \n')
fprintf('%i %i %d %s \n',degreelongp,minutelongp,secondlongp,hemilongp)
fprintf('%i %i %d %s \n',degreelatp,minutelatp,secondlatp,hemilatp)
%Calculating the error
sum=0;
total=0;
for k=1:sizea
for l=1:sizeb
if pab(k,l)>0.1*mostprobablepoint
total=total+1;
x1=re*cos(b(l))*cos(a(k));
y1=re*cos(b(l))*sin(a(k));
z1=re*sin(b(l));
x2=re*cos(centerb)*cos(centera);
y2=re*cos(centerb)*sin(centera);
z2=re*sin(centerb);
sum=sum+((x1-x2)^2+(y1-y2)^2+(z1-z2)^2)^(1/2);
end
end
end
error=sum/total;
fprintf('Error of the estimation \n')
fprintf('%d',error)