%             ASSIGNMENT --- Detection and Estimation               %
% % %%%  Non-Linear Least Squares --- Bearings-Only Measurement %%% %
%                                                                   %
% Submitted by :      Mansoor Ahsan                                 %
%                     Reg. # 070115                                 %
%                     Date: 11-01-2008                              %
% % *************************************************************** %
clear all
close all
c=[0 15000 30000];                                             % Position x-coordinates of moving platform
d=[0 5000 0];                                                  % Position y-coordinates of moving platform
x=[20000 20000]';                                              % Initial Estimate of x, the target position
variance = [1 1 1];
R=diag(variance);                                              % Covariance matrix
signoise=1;                                                    % standard deviation (so also variance = 1)
wnoise=signoise*randn;                                         % zero-mean white Gaussian measurement noise with variance =1
for i = 1:1:3
    h(i)=atan((x(2)-d(i))/(x(1)-c(i)));                              % eqn 3.4.4-1
end
z=[h(1)+wnoise;h(2)+wnoise;h(3)+wnoise;];                      % Stacked vector of measurements
for j=1:1:5
    for i=1:1:3
            dhda(i)=-(x(2)-d(i))/((x(1)-c(i))^2+(x(2)-d(i))^2);% eqn 3.4.4-7
            dhdb(i)=(x(1)-c(i))/((x(1)-c(i))^2+(x(2)-d(i))^2);% eqn 3.4.4-7
            h(i)=atan((x(2)-d(i))/(x(1)-c(i)));                % eqn 3.4.4-1
    end
    H=[h(1);h(2);h(3)];
    J=[dhda(1) dhdb(1);dhda(2) dhdb(2);dhda(3) dhdb(3)];       % eqn 3.4.4-6
    x1=x + inv(J'*inv(R)*J)*J'*inv(R)*(z-H);                   % eqn 3.4.4-4  
    x=x1;
    xplot(j)=x(1)
    yplot(j)=x(2)
end
plot(xplot,yplot,'')
title('Plot of Target Estimate')
xlabel('X-axis')
ylabel('Y-axis')