function test3d

%Initialize the target position

%randn('state',0);
for k = 1:20,
[Xtar, Xobs] = InitScen(2);
Xh = zeros(6,1);
%Bearing in degress, Range in NM
Rk = [300/5280 d2r(.75) .1*pi/180];% Radar bearing Error, range Error
%initialize target covariance matrix
P = [130 0 0 0 0 0; 0 130 0 0 0 0;0 0 53 0 0 0; 0 0 0 2100 0 0; 0 0 0 0 2100 0;
0 0 0 0 0 21]; U = P; Uk = U;
H = zeros(3,6);
%initial radar measurement and position
N = 100;
T = 1/3600;
%time is in hour, so 1/60 is a minute, 1/3600 is a second
if T == 1/3600
sig = 1;
else
sig = .030;
end
tave = 2*T;
PSD = sqrt(T^3 * sig / tave^3); %power spectral density
Z = get_measurement(Xobs,Xtar,Rk);
Xh= zeros(6,1);
[Xh(1),Xh(2),Xh(3)] = WGSDirect(Xobs(1),Xobs(2),Xobs(3),Z(1),Z(2),Z(3));
Mt=zeros(3,1);TK = zeros(6,N);FK = zeros(6,N);M = zeros(3,N);
nu = zeros(3,1);
figure(1);
r2d = 180/pi;
for j = 1:N,
%Xt0 = GeoCenter(Xtar(1),Xtar(2),Xtar(5));
Xtar = Phi(Xtar,T);
Xobs = Phi(Xobs,T);
Z = get_measurement(Xobs,Xtar,Rk);
[Mt(1),Mt(2),Mt(3)] = WGSDirect(Xobs(1),Xobs(2),Xobs(3),Z(1),Z(2),Z(3));
% start of kalman filter update code
% compute estimated ECEF coords of target
% Xt0 is ECEF coordinate of local NED origin
Xt0 = GeoCenter(Xh(1),Xh(2),Xh(3));
%dHx = ndH(Xobs,Xh);
Oecef = GeoCenter(Xobs(1),Xobs(2),Xobs(3));
% compute NED matrix at estimated target location
endt = Geo2Earth(Xh(1),Xh(2));%contact in North East Down
et = endt(:,1); nt = endt(:,2); dt = endt(:,3);
% compute predicted target location in ECEF
% frame using X += xdot*e + ydot*n
Xtecef = Xt0 + T*(Xh(4)*nt + Xh(5)*et+Xh(6)*dt);
% compute x&y components of Xhat(k|k-1) in enu frame
% note that Xhat(k-1|k-1) is at the origin of the enu frame,
% so this is just Xhat(k|k-1) = velocity x time

Xh(1) = T*Xh(4); Xh(2) = T*Xh(5); Xh(3) = T*Xh(6);

V = Xtecef - Oecef;
% compute zhat(k|k-1) & the measurement matrix H
% zhat ...
mend = Earth2Geo(Xobs(1),Xobs(2));
vend = mend*V;
Rng = norm(vend);% = sqrt(x^2+y^2+z^2)
Rng2 = Rng^4;
Brng = atan2(vend(2),vend(1));
El = asin(vend(3)/Rng);
if Brng < 0,
Brng = Brng + 2*pi;
end
x = vend(1);
y = vend(2);
z = vend(3);
H(1,1) = y/Rng; %drlat = y/Rng
H(1,2) = x/Rng; %drlon= x/Rng;
H(1,3) = z/Rng;
H(2,1) = 1/(x*(1+y^2/x^2)); %dblat = -x/(y^2*(1+x^2/y^2));
H(2,2) = -y/(x^2*(1+y^2/x^2)); %dblon = 1/(y*(1+x^2/y^2));
sum = x^2+y^2+z^2;
den = 1/sqrt(1-z^2/sum);
H(3,1)=-z*y/sum^(1.5)*den;%dedlat
H(3,2) = -z*x/sum^(1.5)*den;%dedlong
H(3,3) = (1/Rng - z^2/sum^(1.5))*den;%dedel
nu = [Z(1) - Rng; Z(2) - Brng; Z(3) - El];
norm_e = 0;
Uk = Cov_Update(U,T,tave,sig);
for i = 1:3,
[Xhi,Uki,alpha,beta] = UD_Update(Xh,Uk,H(i,:),nu(i),Rk(i));
%calculate mean square error, which is a chi2 statistic
norm_e = nu(i)^2 / alpha + norm_e;
Uk = Uki;
Xh = Xhi;
end
U = Uk;
% Xh is now estimated target location in END frame
% with origin at Xt0
% compute ECEF coords of Xh
Xt_ecef = Xt0+ Xh(1)*nt + Xh(2)*et + Xh(3)*dt;
% convert from ECEF to LLA, store lat/lon in Xh(1:2)
[Xh(1),Xh(2),Xh(3)] = Geo2LLA(Xt_ecef);
%EE(j) = norm_e;
% kalman filter is now completed
[r(k,j),b(k,j),e] = WGSInDirect(Xtar(1),Xtar(2),Xtar(3),Xh(1),Xh(2),0);
OK(:,j) = Xobs;
TK(:,j) = Xtar;
FK(:,j) = Xh;
M(:,j) = Mt;

ZZZ(j) = Z(3);
% figure(1);
%Xtar(2)*r2d,Xtar(1)*r2d,'b.-',Xh(2)*r2d,Xh(1)*r2d,'c.-',Mt(2)*r2d,Mt(1)*r2d,'rd
'
end
end
%med = median(RRR);mstd = std(RRR);
r2d = 180/pi;
% figure(1);
plot3(TK(2,:)*r2d,TK(1,:)*r2d,TK(3,:),'b.-',FK(2,:)*r2d,FK(1,:)*r2d,FK(3,:),'c.',M(2,:)*r2d,M(1,:)*r2d,M(3,:),'rd')
% rotate3d on
%figure(2)
inx = 1:N;
plot(inx,FK(4,:),'r',inx,FK(5,:),'g',inx,sqrt(FK(4,:).^2+FK(5,:).^2),'c:')
legend('N Vel','E Vel','Speed')
%%plot(TK(2,:)*r2d,TK(1,:)*r2d,'b*-',OK(2,:)*r2d,OK(1,:)*r2d,'cd-')
%plot(ZZZ)
%-----------------------------------------------------------------------function [Xtar, Xobs,turntime,heading,turnrate] = InitScen(scen)
%Initialize scenario InitScen
if scen == 1,
turntime = 450; heading = d2r(95); turnrate = d2r(2);
Lto =35; Lno = -111.5;Cuso = 300; Speedo = 200;Alto = 15000/5280;
Lt =36; Ln = -110;CusT(1) = 35; SpeedT = 17;Alt = 56/5280;
% Lt =36; Ln = -110;CusT(1) = 155; SpeedT = 17;Alt = 0;
%Lto =35; Lno = -111.5;Cuso = 45; Speedo = 200;Alto = 0;
elseif scen == 2,
Lto =35; Lno = -111.52;Cuso = 0.7; Speedo = 200;Alto = 15000/5280;
Lt =36; Ln = -111.4;CusT(1) = 0.7; SpeedT = 17;Alt = 56/5280;
else
Lto =35.9; Lno = -110.0;Cuso = 315; Speedo = 200;Alto = 15000/5280;
Lt =36; Ln = -111.5;CusT(1) = 45; SpeedT = 17;Alt = 56/5280;
end
Xtar = Initialize(Lt,Ln,Alt,CusT,SpeedT);
Xobs = Initialize(Lto,Lno,Alto,Cuso,Speedo);
%-----------------------------------------------------------------------function Xk = Initialize(Lt,Ln,alt,Cus,Speed);
%Lat,Long in degrees, Alt in NM, Cus in degree, speed in knots
[N,E] = CS2nm(Cus,Speed,Lt);
Xk = [d2r(Lt); d2r(Ln); alt; N; E; 0];
%----------------------------------------------------------------------function [N,E] = CS2nm(Cus,Speed,Lat)
%converts cus/speed in degrees/knots to nm/hr north/east
N = Speed*cos(d2r(Cus));
E = Speed*sin(d2r(Cus));
%----------------------------------------------------------------------function Z = get_measurement(Xobs,Xtar,Rk)
%get the radar measurement, using wgs
Lat1 = Xobs(1); Lon1 = Xobs(2);h1 = Xobs(3);
Lat2 = Xtar(1); Lon2 = Xtar(2);h2 = Xtar(3);

[Rng,Brng,El] = WGSInDirect(Lat1,Lon1,h1,Lat2,Lon2,h2);
if Rk(1) == 0
Z = [Rng,Brng, El];
else
Z = [Rng+randn*Rk(1) Brng+randn*Rk(2) El]';
end
if Rk(1) == 0
[Rng,Brng,El] = WGSInDirect(Lat1,Lon1,h1,Lat2,Lon2,h2);
Z = [Rng Brng El]';
else
if h2 < 0.0189,%100/5280, is altitude < 100 ft, assume on the surface
[Rng,Brng,El] = WGSInDirect(Lat1,Lon1,h1,Lat2,Lon2,h2);
Z = [Rng+randn*Rk(1) Brng+randn*Rk(2) El]';
else
if h2 < 0.5682,% 3000/5280
deviation = randn*0.1515 %std of 800, +/- 1500 ft
while (h2 + deviation ) < 0.0189,%if lower than a hundered feet
deviation = randn*0.1515;
end
h2 = h2 + deviation;
else
deviation = randn*0.1515;
h2 = h2 + deviation;
end
[Rng,Brng,El] = WGSInDirect(Lat1,Lon1,h1,Lat2,Lon2,h2);
Z = [Rng+randn*Rk(1) Brng+randn*Rk(2) El]';
end
end
%----------------------------------------------------------------------function [Rng,Brng,El] = WGSInDirect(Lat1,Lon1,h1,Lat2,Lon2,h2)
V1 = GeoCenter(Lat1,Lon1,h1);
V2 = GeoCenter(Lat2,Lon2,h2);
V3 =V2-V1;
Mned1 = Earth2Geo(Lat1,Lon1);%for elevation
%Mned1 = Earth2Geo(Lat2,Lon2);%for grazing angle
Vned = Mned1 * V3;
Rng = norm(Vned);
if Rng ~= 0,
El = asin(Vned(3)/Rng);
Brng = atan2(Vned(2),Vned(1));
if Brng < 0;
Brng = Brng + 2*pi;
end
else
Brng = 0;
El = 0;
end
%----------------------------------------------------------------------function V = GeoCenter(Lat,Lon,h)
ae = 3.443918300463334e+003;
f = 1/298.257223563;
e2 = (2-f)*f;
Rp = ae./(1 - e2*sin(Lat).^2).^.5;

V = [(Rp+h).*cos(Lat).*cos(Lon);
(Rp+h).*cos(Lat).*sin(Lon);
(Rp*(1-e2)+h).*sin(Lat)];
%----------------------------------------------------------------------function R = Earth2Geo(Lat,Lon)
%Conversion from earth coordinates to geo Coordinates in EastNorthDown
R = [-sin(Lat)*cos(Lon) -sin(Lat)*sin(Lon) cos(Lat);...
-sin(Lon)
cos(Lon)
0; ...
-cos(Lat)*cos(Lon) -cos(Lat)*sin(Lon) -sin(Lat)];
%----------------------------------------------------------------------function [TLat,TLon,TAlt] = WGSDirect(Lat1,Lon1,h1,Rng,Brng,El)
%input Lat/Long/Bearing in radians, rng in nm
%output Lat/Long in radians
f = 1/298.257223563;
e2 = (2-f)*f;
ae = 3.443918300463334e+003;
Geo = GeoCenter(Lat1,Lon1,h1);
Dis = Cart(Rng,Brng,El);
ROT = Earth2Geo(Lat1,Lon1);
%convert Displacement vector from geo to earth
R = Geo + ROT'*Dis;
TXY = sqrt(R(1)^2 + R(2)^2);
TZ = R(3);
Radic = 1;
TLon = atan2(R(2),R(1));
TLat = 0;
TLatNew = atan(TZ/((1-e2)*TXY));
small = 1e-9;
while abs(TLatNew - TLat) > small,
TLat = TLatNew;
sLat = sin(TLat);
cLat = cos(TLat);
Radic = sqrt(1-e2*sLat^2);
F = ae*e2/Radic - TXY/cLat + TZ/sLat;
FPrime = ae*e2^2*sLat*cLat/Radic^3 ...
- TXY*sLat/cLat^2 - TZ*cLat/sLat^2;
Ratio = F/FPrime;
TLatNew = TLat - Ratio;
end
RP = ae/Radic; %This is at the target location
TXY0 = RP * cos(TLatNew);
TZ0 = RP*(1-e2)*sin(TLatNew);
TAlt = sqrt((TXY - TXY0)^2 + (TZ - TZ0)^2);
%----------------------------------------------------------------------function Dis = Cart(Rng,Brng,El)
%convertion from polar to cartisean in NED
Dis = [Rng*cos(Brng)*cos(El);...
Rng*sin(Brng)*cos(El);...
Rng*sin(El)];
%-----------------------------------------------------------------------

function r = d2r(degree)
r = degree*pi/180;
%----------------------------------------------------------------------function Xt1_t = Phi(Xt,t)
%State Propagation
%the state is North in Lat(radians) North,
%
nm/Hour,
%
East
Lon (radians) East
%
nm/Hour
%
Altitud NM
%
Altitude NM/Hour
%
t is time in hours
h = Xt(3);
hdot = Xt(6);
Lat = Xt(1);
Vn = Xt(4);
VLat = dNorth(Lat,h,Vn);
Ve = Xt(5);
VLon = dEast(Lat,h,Ve);
Lon = Xt(2);
Xt1_t = [Lat+VLat*t; Lon+VLon*t;h+hdot*t; Vn;Ve;hdot];
%----------------------------------------------------------------------function V = dNorth(Lat,h,Vn)
%Vn is in arc minutes north per hour
%V is in radians north per hour
V = Vn/(Rm(Lat)+h);
%----------------------------------------------------------------------function r = Rm(Lat)
f = 1/298.257223563;
e2 = (2-f)*f;
ae = 3.443918300463334e+003;
r = ae*(1-e2)./(1-e2*sin(Lat).^2).^1.5;
%----------------------------------------------------------------------function r = Rp(Lat) %Lat in radians
ae = 3.443918300463334e+003;
f = 1/298.257223563;
e2 = (2-f)*f;
r = ae./(1 - e2*sin(Lat).^2).^.5;
%----------------------------------------------------------------------function V = dEast(Lat,h,Ve)
%Ve is in arc Minutes per hour east
%V is in Longitude east
V = Ve.*sec(Lat)./(Rp(Lat)+h);
%----------------------------------------------------------------------function R = Geo2Earth(Lat,Lon)
%Conversion from geo to earth local
R = [-sin(Lat)*cos(Lon)
-sin(Lon)
-cos(Lat)*cos(Lon);...
-sin(Lat)*sin(Lon)
cos(Lon)
-cos(Lat)*sin(Lon);...
cos(Lat)
0
-sin(Lat)];

%----------------------------------------------------------------------function [U] = Cov_Update(U,dt,tave,sig)

%FUDU'F + Q update of the state covariance matrix, ie the Plant
%noise, number of positional states = N, for longitude,
PSD = sqrt(dt^3 * sig / tave^3); %power sqectral density
N = length(U)/2;
%N = 2;
for i = 1:N,
for j = i:N,
if(i==j)
U(i,j+N) = U(i,j+N) + dt;
elseif(i < j)
U(i,j+N) = U(i,j+N) + dt*U(i+N,j+N);
end
end
end
for i = 1:N,
A = zeros(2*N,1);
A(N+i) = PSD;
A(i) = dt*PSD/2;
U= UDU_update(U,A);
U(i,i) = U(i,i)+dt^2*PSD^2/12;
end
%----------------------------------------------------------------------function [x,U,alpha,beta] = UD_Update(x,U,H,z,r)
%This decriments the covariance and updates the gain
%x is the apriori estimate
%U is upper triangular matrix with D stored on the diag.,P = UDU'
%H is the observation coefficeinet, ie row i of jacobian
%z is the Observation residual,
%r is the observation variance
N = length(U);
%n = 4;
for j= N:-1:2,
for k = 1:j-1,
H(j) = H(j) + U(k,j)*H(k);
end
b(j) = U(j,j)*H(j);
end
b(1) = U(1,1)*H(1);
% b = DU'H, and H = U'H have been computed
alpha = r + b(1)*H(1);
gamma = 1.0/alpha;
U(1,1) = r * gamma * U(1,1);
for j = 2:N,
beta = alpha;
alpha = alpha + b(j)*H(j);
%alpha = HPH' + Rc, or the normalized covariance
lambda = -H(j)*gamma;
gamma = 1.0/alpha;
U(j,j) = beta * gamma * U(j,j);
for i = 1:j-1;
beta = U(i,j);
U(i,j) = beta + b(i) * lambda;
b(i) = b(i) + b(j) * beta;
end
end

z = z * gamma;
for j=1:N,
x(j) = x(j) + b(j) * z;
end
%-----------------------------------------------------------------------%-function [U] = UDU_update(U,a)
%UbDbUd' = UDU' + caa', include process noise into
%the U-D factored covarinace, c is a scalar, a in n*1 vector
N = length(U);
%N = 4;
for i = 1:N
D(i) = U(i,i);
end
for j = N:-1:2
s = a(j);
bigd = D(j) + s^2;
b = 1.0/bigd;
beta = s*b;
c = b*D(j);
U(j,j) = bigd;
for i = j-1:-1:1
a(i) = a(i) - s*U(i,j);
U(i,j) = U(i,j)+ beta*a(i);
end
end
U(1,1) = D(1) + c*a(1)^2;
%---------------------------------------------------------------------%---function [lat,lon,alt] = Geo2LLA(X)
%covert geo (earth center earth fix) to lat/lon
%ae = 6378137;
ae = 3.443918300463334e+003;
f = 1/298.257223563;
e2 = (2-f)*f;
TXY = sqrt(X(1)^2 + X(2)^2); %down range
TZ = X(3); %the height
lon = atan2(X(2),X(1));%get the longitude
%initial guess at the latitude
lat = atan2(TZ,TXY);
R = Rp(lat); %local radius of curviture
alt = TXY/cos(lat) - R;
error = 100;
while error > .0001
R = Rp(lat);
newlat = atan2(TZ/(1-e2*R /(R+alt)),TXY);
newalt = TXY/cos(lat) - R;
error = sqrt((newalt-alt)^2 + (newlat-lat)^2*ae^2);
alt = newalt;
lat = newlat;
end
%-------------------------------------------------------------------%------

%-----------------------------------------------------------------