%
% PURPOSE: Simulate the effect of hard and soft iron on a compass.
%
% DATE:    1 May 2020; rsm
%
% NOTES:   Using NED.
% 
 psi = [0:200]'/200*2*pi;
 B = 500;                        %MilliGuass, Earth field strength
 ae = 1;                         %Major axis
 be = .5;                        %Minor axis
%be = 1;                         %Minor axis
 theta = pi/4;                   %Ellipse rotation angle
%theta = 0;                      %Ellipse rotation angle
 hard_offset = [100 30];         %MilliGauss, additive field in the horizontal plane
%hard_offset = [0 0];            %MilliGauss, additive field in the horizontal plane
 
 b_in  = B*[cos(psi) sin(psi)];
 
 b_out = B*[ae*cos(psi) be*sin(psi)];
 
 b_out = b_out*[cos(theta)   sin(theta);
               -sin(theta)   cos(theta)];

 b_out = b_out + hard_offset;
 
 
% b_out = B*[ae*cos(psi) be*sin(psi)] + hard_offset;
 
 
 compass = atan2(b_out(:,2),b_out(:,1));
%compass = atan(b_out(:,2)./b_out(:,1));
 indx = find(compass<0);
 compass(indx) = compass(indx)+2*pi;
 

if(~exist('position'))
  position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;
end

co = [0 0 1;
      0 0.5 0;
      1 0 0;
      0 0.75 0.75;
      0.75 0 0.75;
      0.75 0.75 0;
      0.25 0.25 0.25];
set(groot,'defaultAxesColorOrder',co)

if( exist( 'fig1' ) ~= 1 )
  fig1 = figure;
  figure(fig1);
else
  figure(fig1);
  clf
end
set( fig1, 'Name', 'Effect of Hard Iron', 'DeleteFcn', 'clear fig1' );
set(gcf,'position',position)
set(gcf,'colormap',jet)
if(~exist('fontsize')) fontsize = 20;end
plot(b_in(:,2), b_in(:,1), 'b',b_out(:,2), b_out(:,1),'r');
axis equal
set(gca,'fontsize',fontsize);
grid on
hold on
title('Magnetometer Input and Output')
xlabel('milli-Gauss');
ylabel('milli-Gauss');
legend('Perfect Magnetometer','With Hard and Soft Iron Error')

if( exist( 'fig2' ) ~= 1 )
  fig2 = figure;
  figure(fig2);
else
  figure(fig2);
  clf
end
set( fig2, 'Name', 'Effect of Hard Iron', 'DeleteFcn', 'clear fig2' );
set(gcf,'position',position)
set(gcf,'colormap',jet)
if(~exist('fontsize')) fontsize = 20;end
plot(psi*180/pi, psi*180/pi,'b',psi*180/pi,compass*180/pi,'r')
axis equal
set(gca,'fontsize',fontsize);
grid on
hold on
xlabel('Angle of Vehicle, Deg');
ylabel('Measured Angle, Deg');
title('Effect of Hard and Soft Iron on Compass Heading')
legend('Perfect Compass','With Hard and Soft Iron Error')
