%% NOTES
% This solution is currently good for rotations where measurements of yaw
% differ by less than 45 degrees.

%% INPUT


clear; close all;

% The origin (or vehicle frame)
Oyaw   = 0;
Opitch = 0;
Oroll  = 0;

% Sensor A = Sprint or Octans

% offsets to frame in zero position (collected from sensor B)
Ayawoffset = 0;
Apitchoffset = 0;
Arolloffset = 0;

Ayaw   = Oyaw   + Ayawoffset;
Apitch = Opitch + Apitchoffset;
Aroll  = Oroll  + Arolloffset;

% offsets to frame in zero position
%Byawoffset = 0;
%Bpitchoffset = 0;
%Brolloffset = 0;

Byaw   = 0;
Bpitch = 10;
Broll  = 0;


%% Rotation Matrix Encoding

Ra = rot(Ayaw,Apitch,Aroll);
Rb = rot(Byaw,Bpitch,Broll);

Rab = Ra*Rb';

framePitch = asind(Rab(3,1))
netRoll  = atand(-1*Rab(3,2)/Rab(3,3))
netYaw   = atand(-1*Rab(2,1)/Rab(1,1))


%% Inverse solution of expected values for Kearfott values for given Rab
clear;
fYaw = 0;
fRoll = 0:1:20;
fPitch = 0:90;
INSy = zeros(size(fPitch));
INSp = zeros(size(fPitch));
INSr = zeros(size(fPitch));

% Assume the frame in zero position is inline in pitch and roll with the 
% Vehicle INS (Sensor A)

for m = 1:length(fRoll);
Ra = rot(fYaw,0,fRoll(m));

for n = 1:length(fPitch)

    Rab = rot(fYaw,fPitch(n),fRoll(m));
    Rb = (Rab/(Ra))';
    INSy(m,n) = atand(-1*Rb(2,1)/Rb(1,1));
    INSr(m,n) = atand(-1*Rb(3,2)/Rb(3,3));
    INSp(m,n) = asind(Rb(3,1));

end
end

fPitch2d = repmat(fPitch,length(fRoll),1);

plot(fPitch2d',INSp');
xlabel('Frame Pitch ({\circ})');
ylabel('Frame INS Pitch Values');
grid on;

