%PROGRAM NAME: ufov2.m   (Updated 11/15/17)
% ***Originally located in 2011/Homework folder***
% Generate random 2-D path of a ufo
n = 200; %Integer time maximum
tvec = 1:n;
% *** Nominal Circular Path ***
cx1 = 100; cx2 = 100; %Orbit Center
T = 200;F = 1/T; % T=200 second period
r = 60; % nominal radius
Rnom = r*ones(1,n); % Nominal Radius as fcn of t
Th = 2*pi*F*tvec;% Nominal Angle as fcn of t
RTh = [Rnom ; Th]; %<- Known input to KF 
%--- Convert nominal path to Cartesean coordinates ---
x1c = cx1 + r*cos(2*pi*F*tvec);
x2c = cx2 + r*sin(2*pi*F*tvec);
Xc = [x1c;x2c]; 
figure(1)   % ***FIGURE 1 ***
plot(x1c,x2c,'--k','LineWidth',2)
title('UFO Nominal Orbit')
xlabel('x1')
ylabel('x2')
grid
legend('nominal orbit')
npix = 200;
axis([0 npix 0 npix]);
hold on
pause
% *** Generate radius perturbation (dr) AR(1) process & Path ***
dr = zeros(1,n);
stddr = 0.1*r; % dr standard deviation
b = .02; % dr BW in Hz
a = exp(-2*pi*b); %AR(1) bw parameter
u = stddr*(1-a^2) * randn(1,n);
dr(1) = stddr*randn(1,1);
x = zeros(2,n);
x(1,1) = cx1 + (r+dr(1))*cos(2*pi*F);
x(2,1) = cx2 + (r+dr(1))*sin(2*pi*F);
for t = 2:n
    dr(t) = a*dr(t-1) + u(t);
    x(1,t) = cx1 + (r+dr(t))*cos(2*pi*F*t);
    x(2,t) = cx2 + (r+dr(t))*sin(2*pi*F*t);
end
for t=2:n
    xx1 = x(1,t);
    xx2 = x(2,t);
    plot(xx1,xx2,'ko')
    title('Nominal and Perturbed (substance-induced?) Orbits')
    legend('nominal','perturbed(ok)')
    pause(.02)
end
pause
% Generate Camera Images
X = round(x); % Round ufo position to integer coordinates (pixel resolution)
plot(X(1,:),X(2,:),'*g')
title('Nominal , Perturbed(o) & Pixelated(*g) Orbits')
legend('nominal','perturbed(ok)','pixelated(g*)')
pause
UFO = zeros(npix,npix); % Define Image Pixels
Z = zeros(npix,npix); %Initialize camera image array for any time t.
Zx = 1:npix; Zy = 1:npix;   % pixel array locations
for t = 1:n
    m1 = X(1,t); m2 = X(2,t); % pixel location of UFO at time t.
    UFO(m1,m2) = 100; % pixel value of UFO at time t.
end
% Now the array UFO contains the UFO pixel LOCATIONS for t = 1:n
%-----------------------------------------------------------
% Construct the camera image sequence + a single frame of white noise
% The logic for a SINGLE frame is that the background noise is STATIC
% throughout the tracking period.
sn = 3;
Z = UFO + sn*randn(npix,npix);
figure(2)
contour(Zx,Zy,Z,5)
title('Pixelated path + Static Pixel Noise (e.g. stars/planets)')
pause
% Generate Location measurement Process
dw = 4; % Camera Local window size = 2*dw+1
z = [];
for t = 1:n
    zt = ZZWINv2(Xc(:,t),dw,Z); %**My function ZZWINv2 MUST be resident**
z = [z zt];
end
figure(1)
z1 = z(1,:); z2 = z(2,:);
plot(z1,z2,'+r')
title('nominal, perturbed, pixeled & measured orbits')
legend('nominal','perturbed(ok)','pixelated(*g)','measured(+r')
pause
%===========================================
% Convert to polar coordinates re: (cx1,cx2)
Cx1 = cx1*ones(1,n); Cx2 = cx2*ones(1,n);
zr = ((z1 - Cx1).^2 + (z2 - Cx2).^2).^.5;
zth = atan2((z2-Cx2),(z1-Cx1));
zrth = [zr ; zth];
% Design KF for Prediction
I = eye(2,2);
Phi= [1 0; 0 1];
H = [1 0;0 1];
% Cov Matrix for x is obtained from trial+error:
sru2=10^-10; % <==== Radius DRIVING NOISE VARIANCE 
sthu2 = 10^-10; %<=== Theta DRIVING NOISE VARIANCE
Q=[sru2 0 ; 0 sthu2];
sv = 10^-10; 
R=sv^2*I;
%=====================================
% EKF Initial Conditions
xvecm = RTh(:,1);
Pm = [stddr^2 0 ; 0 .01];
%====================================
xvec = xvecm;
% EKF Loop
for t=1:n-1
% Generate Zoomed Image
K=Pm*H'*(H*Pm*H' + R)^(-1);
zm = H*xvecm;
xvechatk = xvecm + K*(zrth(:,t) - zm);
xvec = [xvec xvechatk];
P =(I - K*H)*Pm;
xvecm = Phi*xvechatk + RTh(:,t+1); %<--- NOTE the inclusion of the INPUT
Pm=Phi*P*Phi' + Q;
end
% =================================================================
rhat=xvec(1,:);
thhat=xvec(2,:);
figure(3)
rr = Rnom + dr;
subplot(2,1,1),plot(tvec,rr,'b',tvec,rhat,'r*')
title('True Radius(blue) & KF Radius Tracking(red)')
grid
subplot(2,1,2),plot(tvec,Th,'b',tvec,thhat,'r*')
title('True Angle(blue) & KF Angle Tracking(red)')
xlabel('Time')
grid
%=====================
    
