%% LoadFile
% The easiest way to read in the XTRA or BASE .csv is to read it in as
% a table.
AIM_Data=readtable('E190_9SEP2017_J825R_1_XTRA.csv');
% You have to actually look at the column headings to make sure
% you have the correct column names.
% Because the AIM XTRA uses different sample rates for the different
% sensors, most columns (except for the z-axis accelerometer and associated
% time) will have NaNs for much of the column. They show up once the actual
% column data cease. THe easiest way to remove them is with a conditional
% using isfinite.

% The data in a column heading may be accessed by appending with a dot, ".",
% and either the column name, e.g., .time, or the column number in
% parentheses, e.g., .(1). The example below shows how to plot two columns
% using either method of addressing.


%% SearchApogee
% Find the apogee time
accelTimePost=AIM_Data.time_5(AIM_Data.time_5>=0);
accelZPost=AIM_Data.acceleration(AIM_Data.time_5>=0);
plot(accelTimePost,accelZPost)
title('z-axis acceleration')
xlabel('time (s)')
ylabel('a_z (g)')

%% SetApogee
apogeeTime = 24.6; % Determined by looking at the above plot and zooming in.
% A clever person may be able to automate the selection process.
timeArr = 0:0.001:apogeeTime; % The Z accelerometer has an approximate 
% sample rate of 100 SPS, or a period of 0.01 s. Using a precise time
% array makes a lot of things simpler. We'll resample later using timeArr.

% Table data are column vectors.
% You should transpose most table data before using.

%% GetMagnetometerData
% The magnetometer chip is a left-handed coordinate system. To
% align X, Y, and Z with ENU, we need to flip the sign on X and Y.
timeMag=transpose(AIM_Data.time_9(isfinite(AIM_Data.time_9)));
magX = -transpose(AIM_Data.MagX(isfinite(AIM_Data.MagX))); % Flip X
magY = -transpose(AIM_Data.MagY(isfinite(AIM_Data.MagY))); % Flip Y
magZ =  transpose(AIM_Data.MagZ(isfinite(AIM_Data.MagZ))); % No flip
plot(timeMag,magX, timeMag,magY, timeMag,magZ)
title('Raw Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% ScaleMagData
magXMax = max(magX);
magXMin = min(magX);
magYMax = max(magY);
magYMin = min(magY);
magZMax = max(magZ);
magZMin = min(magZ);
magXsc = (2*magX-(magXMax+magXMin))/(magXMax-magXMin);
magYsc = (2*magY-(magYMax+magYMin))/(magYMax-magYMin);
magZsc = (2*magZ-(magZMax+magZMin))/(magZMax-magZMin);
magMagsc = sqrt(magXsc.^2 + magYsc.^2 + magZsc.^2);
plot(timeMag,magXsc, timeMag,magYsc, timeMag,magZsc, timeMag,magMagsc)
title('Scaled Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z','mag')

%% RenormalizeMagData
% The previous graph shows that the normalized magnitude isn't always 1.00
% so we renormalize so that all of the vectors have magnitude 1.
magXsc = magXsc./magMagsc;
magYsc = magYsc./magMagsc;
magZsc = magZsc./magMagsc;
plot(timeMag,magXsc, timeMag,magYsc, timeMag,magZsc)
title('Scaled Renormalized Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% Get Magnetometer Initial Orientation
% Get the magnetometer pre-launch data and calculate the baselines.
% We've skipped getting the pre-launch data as separate variables.
magX0 = mean(magXsc(timeMag<0));
magY0 = mean(magYsc(timeMag<0));
magZ0 = mean(magZsc(timeMag<0));
mag0 = norm([magX0 magY0 magZ0]);
magX0 = magX0/mag0;
magY0 = magY0/mag0;
magZ0 = magZ0/mag0;
magVec0 = [magX0; magY0; magZ0];

% Include the earth magnetic vector from https://www.ngdc.noaa.gov/geomag-web/#igrfwmm
% Use the pre-launch GPS Baselines from above to look up the vectors.
% N 34.4962, W 116.9593, MSL 861.9322 m, 9 July 2017
magN = 23491.6;
magE = 4926.6;
magD = 40795.3;
magUp = - magD;
magEarthVec = [magE; magN; magUp];
magEarthNorm = norm(magEarthVec);
magEarthVecNorm = magEarthVec/magEarthNorm;

% Calculate the variance in roll angle measurements
rollVar = var(atan2(magYsc(timeMag<0),magXsc(timeMag<0)));

%% ResampleMag
% Get the magnetometer data between launch and apogee by resamling at the timeArr
% values.
magXrs = interp1(timeMag,magXsc,timeArr,'pchip');
magYrs = interp1(timeMag,magYsc,timeArr,'pchip');
magZrs = interp1(timeMag,magZsc,timeArr,'pchip');

% Create the set of local measured magnetometer vectors. They are column
% vectors of the form [mx; my; mz] and the column index corresponds to the
% index of a given timeArr value
magVec = [magXrs; magYrs; magZrs]; % Set of magnetometer vectors
%renormalize again
n = length(timeArr);
for i = 1:n
    magVec(1:3,i) = magVec(1:3,i)/norm(magVec(1:3,i));
    roll(i) = atan2(magVec(2,i),magVec(1,i)); % May want to move this
    %tilt(i) = atan2(magVec(3,i),norm([magVec(2,i) magVec(1,i)]));
end
plot(timeArr,magVec(1,1:n),timeArr,magVec(2,1:n),timeArr,magVec(3,1:n))
title('Scaled and Normalized Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% RollVariance
% We're trying to determine typical jump sizes from one time step to the
% next.
droll = (unwrap(roll(2:n))-unwrap(roll(1:n-1)));
plot(timeArr(2:n),droll);
title('numerical derivative of roll')
xlabel('time (s)')
ylabel('droll/dt')
actRollVar = var(droll);


%% GetGyroData
% Getting IMU data. Note the transposition and NaN removal.
timeGyro=transpose(AIM_Data.time(isfinite(AIM_Data.time)));
gyroX = transpose(AIM_Data.GyroX(isfinite(AIM_Data.GyroX)));
gyroY = transpose(AIM_Data.GyroY(isfinite(AIM_Data.GyroY)));
gyroZ = transpose(AIM_Data.GyroZ(isfinite(AIM_Data.GyroZ)));
plot(timeGyro,gyroZ, timeGyro,gyroX, timeGyro,gyroY)
title('Gyro Data')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_z', '\omega_x', '\omega_y')

%% ConvertUnits
% Convert IMU data to radians per second and meters per second.
gyroX=gyroX*pi/180;
gyroY=gyroY*pi/180;
gyroZ=gyroZ*pi/180;

%% Resample
% Get the gyro pre-launch data and calculate the baselines or offsets.
% We've skipped getting the pre-launch data as separate variables.
gyroXbase = mean(gyroX(timeGyro<0));
gyroYbase = mean(gyroY(timeGyro<0));
gyroZbase = mean(gyroZ(timeGyro<0));

% Get the variance of the roll gyro
rgVar = var(gyroZ(timeGyro<0));

% Get the gyro data between launch and apogee by resamling at the timeArr
% values.
gyroXrs = interp1(timeGyro,gyroX,timeArr,'pchip');
gyroYrs = interp1(timeGyro,gyroY,timeArr,'pchip');
gyroZrs = interp1(timeGyro,gyroZ,timeArr,'pchip');

% Create the set of local measured rotation-rate vectors. They are column
% vectors of the form [wx; wy; wz] and the column index corresponds to the
% index of a given timeArr value
rotRateVec = [gyroXrs-gyroXbase; gyroYrs-gyroYbase; gyroZrs-gyroZbase];

% Get z-gyro variance
% We're trying to determine typical jump sizes from one time step to the
% next.
dGyroZ = (gyroZrs(2:n) - gyroZrs(1:n-1));
plot(timeArr(2:n),dGyroZ)
title('numerical derivative of gyro z')
xlabel('time (s)')
ylabel('dwz/dt')
actGyroZVar = var(dGyroZ);

%% RollPitchYawAngle
rGyro(1) = roll(1);
rGyrocd(1) = roll(1);
pGyro(1) = 0; % tilt(1); % Fix starting angle
yGyro(1) = 0; % tilt(1); % Fix starting angle
for i = 1:n-1
    rGyro(i+1) = rGyro(i) + (timeArr(i+1)-timeArr(i))*(gyroZrs(i)-gyroZbase);
    rGyrocd(i+1) = rGyrocd(i) + 0.5*(timeArr(i+1)-timeArr(i)) * ...
        ((gyroZrs(i+1)-gyroZbase)+(gyroZrs(i)-gyroZbase));
    pGyro(i+1) = pGyro(i)+ (timeArr(i+1)-timeArr(i))*(gyroXrs(i)-gyroXbase);
    yGyro(i+1) = yGyro(i)+ (timeArr(i+1)-timeArr(i))*(gyroYrs(i)-gyroYbase);
end

%% RollSmoothing
%bestWeight = 0.5;
%bestWeight = fminsearch(smooth(roll, rollVar ,gyroZrs, ...
%rgVar, 0.01, roll(1), bestWeight),0.5);
roll = unwrap(roll);
gyTry = (gyroZrs - gyroZbase);
[res, theta] = smooth(roll, rollVar ,gyTry, rgVar, 0.001, roll(1), 0.01);
plot(timeArr, 180/pi*roll, timeArr, 180/pi*rGyro, timeArr, 180/pi*theta, '.')
title('Roll Smoothing')
xlabel('time (s)')
ylabel('Roll Angle (°)')
legend('roll', 'rGyro', 'smooth')

%% RollKalmanFilter
Fk = [1 0; 0.01 1];
FkT = transpose(Fk);
Hk = [1 0; 0 1];
HkT = transpose(Hk);
Pk1k1(1:2,1:2,1) = [0 0; 0 0];
Kk(1:2,1:2,1) = [0 0; 0 0];
xk1k1(1:2,1) = [gyroZrs(1); roll(1)];
Rk = [rgVar 0; 0 rollVar];
Qk = [actGyroZVar 0; 0 actRollVar]; % Have to guess these values for now
II = [1 0; 0 1];
yk(1) = 0;
ykk(1) = 0;
for i = 1:n-1
    % Predict
    xkk1(1:2,i) = Fk*xk1k1(1:2,i);
    Pkk1(1:2,1:2,i) = Fk*Pk1k1(i)*FkT + Qk;
    % Update
    zk(1:2,i+1) = [gyroZrs(i+1) roll(i+1)];
    yk(1:2,i+1) = zk(1:2,i+1) - Hk*xkk1(1:2,i);
    Sk(1:2,1:2,i+1) = Rk + Hk*Pkk1(1:2,1:2,i)*HkT;
    Kk(1:2,1:2,i+1) = Pkk1(1:2,1:2,i)*HkT*inv(Sk(1:2,1:2,i+1)); % Kalman gain
    xk1k1(1:2,i+1) = xkk1(1:2,i) + Kk(1:2,1:2,i+1)*yk(1:2,i+1);
    Pk1k1(1:2,1:2,i+1) = (II - Kk(1:2,1:2,i+1)*Hk)*Pkk1(1:2,1:2,i)* ...
        transpose(II - Kk(1:2,1:2,i+1)*Hk)...
        + Kk(1:2,1:2,i+1)*Rk*transpose(Kk(1:2,1:2,i+1));
    ykk(1:2,i+1) = zk(1:2,i+1) - Hk*xk1k1(1:2,i+1); % Measurement post-fit residual
end
rgKalman = xk1k1(2,1:n); % The roll angle put out by the Kalman Filter
rdotKalman = xk1k1(1,1:n); % The roll rotation rate from the Kalman Filter.
k11(1:n) = Kk(1,1,1:n);
k12(1:n) = Kk(1,2,1:n);
k21(1:n) = Kk(2,1,1:n);
k22(1:n) = Kk(2,2,1:n);
resGyro(1:n) = ykk(1,i+1);
resMag(1:n) = ykk(2,i+1);

%% KalmanGainPlot
plot(timeArr, k11, timeArr, k12, timeArr, k21, timeArr, k22)
title('Kalman Gains')
xlabel('time (s)')
ylabel('Kalman Gain')
legend('k11', 'k12', 'k21', 'k22')

%% ResidualRatePlot
plot(timeArr, 180/pi*resGyro)
title('Rotation Rate Residual')
xlabel('time (s)')
ylabel('Residual (°/s)')

%% ResidualAnglePlot
plot(timeArr, 180/pi*resMag)
title('Rotation Angle Residual')
xlabel('time (s)')
ylabel('Residual (°)')

%% KalmanVsWeightedPlot
plot(timeArr, 180/pi*theta, timeArr, 180/pi*rgKalman,'.')
title('Kalman vs. Weighted Calc')
xlabel('time (s)')
ylabel('Roll Angle (°)')
legend('weighted', 'Kalman')

%% RollPlot
plot(timeArr, 180/pi*roll, timeArr, 180/pi*rGyro, timeArr, 180/pi*rgKalman,'.')
title('Unwrapped Roll Angle in Degrees')
xlabel('time (s)')
ylabel('Roll Angle (°)')
legend('magnetometer', 'rate gyro', 'Kalman')

%% RollDifference
rollDif = unwrap(roll)-unwrap(rGyro);
plot(timeArr, 180/pi*rollDif)
title('Difference between Magnetometer and Gyro in Degrees')
xlabel('time (s)')
ylabel('Difference in Roll Angle (°)')

%% KalmanDifference
rollDif = roll-rgKalman;
plot(timeArr, 180/pi*rollDif)
title('Difference between Magnetometer and Kalman in Degrees')
xlabel('time (s)')
ylabel('Difference in Roll Angle (°)')

%% KalmanDifferenceII
rollDif = rGyro-rgKalman;
plot(timeArr, 180/pi*rollDif)
title('Difference between Gyro and Kalman in Degrees')
xlabel('time (s)')
ylabel('Difference in Roll Angle (°)')

%% RdotPlot
plot(timeArr, 180/pi*gyroZrs, timeArr, 180/pi*rdotKalman)
title('Roll Rate in Degrees')
xlabel('time (s)')
ylabel('Roll Rate (°/s)')
legend('rate gyro', 'Kalman')

%% RdotKalmanDifference
rdotDif = gyroZrs-rdotKalman;
plot(timeArr, 180/pi*rdotDif)
title('Difference between Rate Gyro and Kalman in °/s')
xlabel('time (s)')
ylabel('Difference in Roll Rate (°/s)')
