% This program estimates the MRPs from two vector observations
% using nonlinear least squares.

% Written by John L. Crassidis 9/13

% Other Required Routines: none

% True MRP and Attitude
p=[sqrt(2)/2;0;0]/(1+sqrt(2)/2);
pc=[0 -p(3) p(2)
   p(3) 0  -p(1)
  -p(2) p(1) 0];
a=eye(3)+(8*pc^2-4*(1-norm(p)^2)*pc)/(1+norm(p)^2)^2;

% True Body and Reference Vectors
b1=[0;0;1];b2=[0;0.1;sqrt(1-0.1^2)];
r1=a'*b1;r2=a'*b2;

% Noise Covariances
sig=0.001*pi/180;
rcov1=sig^2*(eye(3)-b1*b1');
rcov2=sig^2*(eye(3)-b2*b2');

% Number of Monte Carlo Runs and Variable Storage
monte_run=1000;
sig3=zeros(monte_run,3);
pest=zeros(monte_run,3);
att_err=zeros(monte_run,3);
number_iter=zeros(monte_run,1);

% Maximum Iterations
max_iter=100;

% Monte Carlo Loop
for j=1:monte_run

% Initial Guess
p0=[0;0;0];

% Body Measurements
[u1,r_diag]=eig(rcov1);
v1_uncorr=randn(1,3)*r_diag.^(0.5);
v1=u1*v1_uncorr';
b1m=b1+v1;b1m=b1m/norm(b1m);

[u2,r_diag]=eig(rcov2);
v2_uncorr=randn(1,3)*r_diag.^(0.5);
v2=u2*v2_uncorr';
b2m=b2+v2;b2m=b2m/norm(b2m);

% Nonlinear Least Squares Iteration
delta_x=10;j_iter=0;
while norm(delta_x)>1e-9

% Estimated Outputs    
pcest=[0 -p0(3) p0(2)
   p0(3) 0  -p0(1)
  -p0(2) p0(1) 0]; 
aest=eye(3)+(8*pcest^2-4*(1-norm(p0)^2)*pcest)/(1+norm(p0)^2)^2;
b1est=aest*r1;
b2est=aest*r2;

% Form H Matrix
b1estc=[0 -b1est(3) b1est(2)
       b1est(3) 0  -b1est(1)
      -b1est(2) b1est(1) 0];
b2estc=[0 -b2est(3) b2est(2)
       b2est(3) 0  -b2est(1)
      -b2est(2) b2est(1) 0];
h1=4*b1estc;
h2=4*b2estc;
  
h=[h1;h2];

% Compute Corrections
delta_y=[b1m-b1est;b2m-b2est];
delta_x=pinv(h'*h)*h'*delta_y;

% Update Initial Guess
p0=((1-p0'*p0)*delta_x+(1-delta_x'*delta_x)*p0-2*cross(delta_x,p0))/(1+(delta_x'*delta_x)*(p0'*p0)-2*delta_x'*p0);

% Check Iterations
j_iter=j_iter+1;
 if j_iter==max_iter 
  disp('Maximum Iterations Reached')
  break
 end
 
end

% Store Estimated MRP and Number of Iterations
pest(j,:)=p0';
number_iter(j)=j_iter;

% Estimated Outputs (after iterations are complete)   
pcest=[0 -p0(3) p0(2)
   p0(3) 0  -p0(1)
  -p0(2) p0(1) 0]; 
aest=eye(3)+(8*pcest^2-4*(1-norm(p0)^2)*pcest)/(1+norm(p0)^2)^2;
b1est=aest*r1;
b2est=aest*r2;

% H Matrix (after iterations are complete)
b1estc=[0 -b1est(3) b1est(2)
       b1est(3) 0  -b1est(1)
      -b1est(2) b1est(1) 0];
b2estc=[0 -b2est(3) b2est(2)
       b2est(3) 0  -b2est(1)
      -b2est(2) b2est(1) 0];
h1=4*b1estc;
h2=4*b2estc;
  
h=[h1;h2];

% Get Error MRP
dp=((1-norm(p0)^2)*p-(1-norm(p)^2)*p0+2*cross(p,p0))/(1+norm(p)^2*norm(p0)^2+2*p'*p0);

% Small Attitude Errors and 3-Sigma Bounds
att_err(j,:)=4*dp;
sig3(j,:)=4*3*sig*diag(pinv(h'*h))'.^(0.5);

end

% Plot Results
clf
t=[1:monte_run]';
subplot(311)
plot(t,-sig3(:,1)*1e6,'r--',t,att_err(:,1)*1e6,'b',t,sig3(:,1)*1e6,'r--')
set(gca,'fontsize',12)
axis([0 monte_run -50 50])
set(gca,'ytick',[-50 -25 0 25 50])
ylabel(['Roll ($\mu$rad)'],'interpreter','latex','fontsize',14)
subplot(312)
plot(t,-sig3(:,2)*1e6,'r--',t,att_err(:,2)*1e6,'b',t,sig3(:,2)*1e6,'r--')
set(gca,'fontsize',12)
axis([0 monte_run -70 70])
set(gca,'ytick',[-70 -35 0 35 70])
ylabel(['Pitch ($\mu$rad)'],'interpreter','latex','fontsize',14)
subplot(313)
plot(t,-sig3(:,3)*1e6,'r--',t,att_err(:,3)*1e6,'b',t,sig3(:,3)*1e6,'r--')
set(gca,'fontsize',12)
axis([0 monte_run -1000 1000])
set(gca,'ytick',[-1000 -500 0 500 1000])
ylabel(['Yaw ($\mu$rad)'],'interpreter','latex','fontsize',14)
xlabel(['Run Number'],'interpreter','latex','fontsize',14)

pause
clf
plot(t,number_iter)
set(gca,'fontsize',12)
ylabel(['Iteration'],'interpreter','latex','fontsize',16)
xlabel(['Run Number'],'interpreter','latex','fontsize',16)

%c=sig^2*inv(eye(3)-b1*b1'+eye(3)-b2*b2');
%sig3_quest=diag(c)'.^(0.5)*3,sig3(1,:)
