%% MARG: Magntometer Acceleration Rate Gyroscope Filter
% Estimate Orientation, Velocity, Position, Gyroscope Bias Error, Accelerometer Bias Error, Geomagnetic Vector, Magnetometer Bias Error
% Donghun Kim
close all;
clear all;
clear all;

% Display format setting
format long eng;

%% UAV Trajectory Loading
% Loading UAV trajectory
load LoggedQuadcopter.mat trajData

% True data
trajOrient = trajData.Orientation;               % True Quaternion
trajVel = trajData.Velocity;                          % True velocity              
trajPos = trajData.Position;                         % True Position
trajAcc = trajData.Acceleration;                 % True Acceleration for accelerometer
trajAngVel = trajData.AngularVelocity;      % True Angular Velocity for rate gyro

% Reference Location: Startt position
refloc = [42.2825 -72.3430 53.0352];

%% Initialize the random number generator used in the simulation of sensor
rng(1);

%% IMU sensor setting
imuFs = 160;                                % IMU processing frequency
gpsFs = 1;                                     % GPS processin frequency

imu = imuSensor('accel-gyro-mag', 'SampleRate', imuFs);

imu.Accelerometer = accelparams(...
                                                    'MeasurementRange', 20, ...     % m/s^2
                                                    'Resolution',0.0023928,...       % m/s^2/LSB
                                                    'ConstantBias',0.19,...                % m/s^2
                                                    'AxesMisalignment',0.5,...         % % 'NoiseDensity',0.004,...             % m/s^s/sqrt(Hz)
                                                    'NoiseDensity',0.004,...            % m/s^s/sqrt(Hz)
                                                    'BiasInstability',0.5);                  % m/se^2
                                                   
imu.Gyroscope   = gyroparams(...
                                               'MeasurementRange', deg2rad(250),... % gyro measurement range 
                                               'Resolution', deg2rad(0.0625),...          % gyro measurement resolution
                                               'ConstantBias',deg2rad(20.125),...
                                               'AxesMisalignment',1.5,...
                                               'NoiseDensity',deg2rad(1.25));
                                           
imu.Magnetometer = magparams(...
                                            'MeasurementRange',1000,...
                                            'Resolution',0.1,...
                                            'ConstantBias',1,...
                                            'NoiseDensity',0.3/sqrt(50));

%% GPS Setting
gps = gpsSensor('UpdateRate', gpsFs);      % GPS object
gps.ReferenceLocation = refloc;
gps.DecayFactor = 0.5;
gps.HorizontalPositionAccuracy = 1.6;       % m
gps.VerticalPositionAccuracy = 1.6;            % m
gps.VelocityAccuracy = 0.1;                        % m/s

%% Initial State Setting
numMeanIndex = 100;
initState = zeros(22, 1);
initState(1:4) = compact(meanrot(trajOrient(1:numMeanIndex)));  % Quaternion 100 sample average
initState(5:7) = mean(trajPos(1:numMeanIndex,:),1);                         
initState(8:10) = mean(trajVel(1:numMeanIndex,:),1);
initState(11:13) = imu.Gyroscope.ConstantBias ./imuFs;
initState(14:16) = imu.Accelerometer.ConstantBias ./ imuFs;
initState(17:19) = imu.MagneticField;
initState(20:22) = imu.Magnetometer.ConstantBias;


%% Multiplicative process noise variance
% time varying process noise for system modeling
GyroscopeNoise = 1e-5;                   % multiplicative process noise variance from gyroscope (rad/sec)^2
AccelerometerNoise = 2;                 % multiplicative accelerometer noise varince from accelerometer (m/sec^2)^2

%% Additive process noise for magnetometer 
% constantly additive noise for system modeling  
GyroscopeBiasNoise = 1e-16;            % additive process noise variance for gyroscope bias (rad/sec)^2
AccelerometerBiasNoise = 2e-4;      % additive process noise variance for accelerometer (m/sec^2)^2

MagnetometerBiasNoise = 1e-10;     % additive process noise for magnetometer bias (uT)^2
GeomagneticVectorNoise = 1e-12;   % additive process noise for geomagnetic vector (uT)^2

OtherAdditiveNoise = 1e-9;

%% Variance for Measurement
Rpos = 2.56;    %GPS position measurement error
Rvel = 0.01;      % GPS velocity measurement error
Rmag = 0.09;   % Magnetometer measurement noise
  
 %% Inital error covariance
P = 1e-9 * ones(22);

%% Simulation time
secondsToSimulate = 140;                                % Simulation time sec
numsamples = secondsToSimulate*imuFs;

loopBound = floor(numsamples);
loopBound = floor(loopBound/imuFs)*imuFs;      % ensure enough IMU Samples

imuSamplePerGPS = (imuFs/gpsFs);
fcnt = 1;

%% State Variables
%  x(1):   q0               quaternion
%  x(2):   q1               quaternion
%  x(3):   q2               quaternion
%  x(4):   q3               quaternion
%  x(5):   pn               North position in NED (m)
%  x(6):   pe               East position in NED (m)
%  x(7):   pd               Down position in NED (m)
%  x(8):   vn               North velocity in NED  (m/s)
%  x(9):   ve               East velocity in NED (m/s)
%  x(10):  vd               Down velocity in NED (m/s)
%  x(11): dax_b         delta angle bias x body frame (radian)
%  x(12): day_b         delta angle bias y body frame (radian)
%  x(13): daz_b         delta angle bias z body frame (radian)
%  x(14): dvx_b         delta velocity bias x body frame (m/s)
%  x(15): dvy_b         delta velocity bias y body frame (m/s)
%  x(16): dvz_b         delta velocity bias z body frame (m/s)
%  x(17): magNavX    North magnetic field in NED (T)
%  x(18): magNavY    East magnetic field in NED (T)
%  x(19): magNavZ    Down magnetic field in NED (T)
%  x(20): magX         Magnetometer bias x body frame
%  x(21): magY         Magnetometer bias y body frame
%  x(22): magZ         Magnetometer bias z body frame
x = initState;

%% Variables
predictQautOrientSavd = quaternion.zeros(loopBound,1);
predictPosSavd = zeros(loopBound,3);
predictVelSavd = zeros(loopBound,3);
predictGyroBiasSavd = zeros(loopBound,3);
predictAccelBiasSavd = zeros(loopBound,3);

prevGyro = [0, 0, 0];
prevAccel = [0, 0, -9.81];

helpFusionFilter = insfilterMARG;

% fusing related data saving
fusedQuatOrientSavd = quaternion.zeros(loopBound,1);
fusedPosSavd =zeros(loopBound,3);
fusedVelSavd = zeros(loopBound,3);

accumGyroSavd = zeros(loopBound,3);
currAccelSavd = zeros(loopBound,3);

Psavd = zeros(loopBound,22);

% Main Routine
while(fcnt <=loopBound)
    
    for ff = 1: imuSamplePerGPS
           
        [currAccel, currGyro, currMag] = imu(trajAcc(fcnt,:), trajAngVel(fcnt, :), trajOrient(fcnt));   % IMU data generating
               
        currAccel = -1 .* currAccel;                                     % converting sign of accelerometer
        dAng = deltaAng(prevGyro, currGyro, 1/imuFs);       % delta angle (radian)
        dVel = deltaVel(prevAccel, currAccel, 1/imuFs);        % delta velocity (m/s)
       
        % Prediction            
        x = stateTransFcn(x, dAng, dVel, 1/imuFs);            
        dfdx = stateTransJacobianFcn(x, dAng, dVel, 1/imuFs);    % df/dx: state transition Jacobian function
        addProcNoise = additiveProcNoise(GyroscopeBiasNoise, AccelerometerBiasNoise,...
                                                            GeomagneticVectorNoise, MagnetometerBiasNoise, OtherAdditiveNoise, imuFs); % additive process noise calculation         
        multProcNoise = procNoiseCov(GyroscopeNoise, AccelerometerNoise, imuFs);                          % multiplicative process noise calculation
        dwdx = procNoiseJacobianFcn(x, multProcNoise);            % dw/dx                
        
        %Pnext = dfdx * P * dfdx' + dwdx + addProcNoise;             % compute prediction covariance        
        Pnext = dfdx * P * dfdx' + addProcNoise;             % compute prediction covariance        
                
        % prediction data save
        predictQautOrientSavd(fcnt,:) = quaternion(x(1), x(2), x(3), x(4));
        predictPosSavd(fcnt,:) = [x(5), x(6), x(7)]; 
        predictVelSavd(fcnt,:) = [x(8), x(9), x(10)]; 
        predictGyroBiasSavd(fcnt,:) = [x(11), x(12), x(13)];
        predictAccelBiasSavd(fcnt,:) = [x(14), x(15), x(16)];
        Psavd(fcnt,:) = diag(Pnext);
                
        % Current to previous transition
        prevAccel = currAccel; 
        prevGyro = currGyro;
        prevMag = currMag;
                
        % Loop count increasing
        fcnt = fcnt +1;
        P = Pnext;
        
    end
    
    % measurement update
    % GPS fusion
    % fusegps(fusionfilt, lla, Rpos, gpsvel, Rvel);   
    [lla, gpsvel] = gps(trajPos(fcnt,:), trajVel(fcnt,:) );    % GPS data generation
    posNED = lla2ned(lla, refloc, 'ellipsoid');                 % convert GPS coordinate to NED coordinate 
      
    z = [posNED, gpsvel].';                    % measurement from GPS
    h = measFcnGPS(x);                      % measurement from State
    dhdx = measJacobianFcnGPS();     % measurement Jacobian
    measNoiseGPS = diag([Rpos, Rpos, Rpos, Rvel, Rvel, Rvel]);    
    [x, P, inno, innoCov] = correctEqn(x, P, h, dhdx, z, measNoiseGPS);
    repairQuat = repairQuaternion([x(1), x(2), x(3), x(4)]);
    x(1:4) = compact(repairQuat);
    
    % Magnetometer fusion
    % fusemag(fusionfilt, mag, Rmag);                
    mx = currMag(1);
    my = currMag(2);
    mz = currMag(3);
    
    z = [mx, my, mz].';
    h = measFcnMag(x);
    dhdx = measJacobianFcnMag(x);
    measNoiseMag = diag([Rmag, Rmag, Rmag]);
    [x, P, inno, innoCov] = correctEqn(x, P, h, dhdx, z, measNoiseMag);
    repairQuat = repairQuaternion([x(1), x(2), x(3), x(4)]);
    x(1:4) = compact(repairQuat);

end

%% Result Display
% Data conversion
trueOrient = eulerd(trajOrient, 'ZYX', 'frame');
predictEulerOrientSavd = eulerd(predictQautOrientSavd, 'ZYX', 'frame');
fusedEulerOrientEuler = eulerd(fusedQuatOrientSavd, 'ZYX', 'frame');

% Orientation compare chart
plotCompareChart(trueOrient, predictEulerOrientSavd, ["Yaw angle","Pitch angle", "Roll angle"],'time','degree',1/imuFs);

% Position compare chart
plotCompareChart(trajPos, predictPosSavd, ["North position","East position", "Down position"],'time','meter',1/imuFs);

% Velocity compare chart
plotCompareChart(trajVel, predictVelSavd, ["North velocity","East velocity", "Down velocity"],'time','m/s',1/imuFs);

% Gyroscope bias estimation 
plotChart(rad2deg(predictGyroBiasSavd).*imuFs, ["Gyro x bias","Gyro y bias","Gyro z bias",],'time','degree',1/imuFs);


% Accelerometer Bias Plot
plotChart(rad2deg(predictAccelBiasSavd).*imuFs, ["Accel. bias x","Accel. bias y","Accel. bias z",],'time','m/s',1/imuFs);


%% 
%DELTANG: Calculate delta angle by trapezoidal accumulating using gyroscope's reading of angular rate
%   prevGyro: previous gyroscope reading
%   currGyro: current gyroscope reading
%   dang: delta angle radian
function dang = deltaAng(prevGyro, currGyro, dt)
    dang = (1/2) * (dt) * (prevGyro + currGyro);
end

%DELTVEL: Calculate delta velocity by trapezoidal accumulating using accelerometer's reading
%   prevAccel: previous gyroscope reading
%   currAccel: current gyroscope reading
%   dvel: delta angle m/s
function dvel = deltaVel(prevAccel, currAccel, dt)
    dvel = (1/2) * (dt) * (prevAccel + currAccel);
end

%STATETRANSFNC: Calculte state one step move forward
%   x: 22 state variable
%   dAng: delta angle
%   dVel: delta velocity
%   dt: sample period
function x = stateTransFcn(x, dAng, dVel, dt)

    q0 = x(1);
    q1 = x(2);
    q2 = x(3);
    q3 = x(4);
    pn = x(5);
    pe = x(6);
    pd = x(7);
    vn = x(8);
    ve = x(9);
    vd = x(10);
    dax_b = x(11);
    day_b = x(12);
    daz_b = x(13);
    dvx_b = x(14);
    dvy_b = x(15);
    dvz_b = x(16);
    magNavX = x(17);
    magNavY = x(18);
    magNavZ = x(19);
    magX = x(20);
    magY = x(21);
    magZ = x(22);
    
    % NED gravitational acceleration
    gravNED = [0, 0, 9.81];
    gnavx = gravNED(1);
    gnavy = gravNED(2);
    gnavz = gravNED(3);
    
    dvx = dVel(1);
    dvy = dVel(2);
    dvz = dVel(3);
    
    dax = dAng(1);
    day = dAng(2);
    daz = dAng(3);
    
    x = [
            q0
            q1
            q2
            q3
            pn + vn * dt
            pe + ve *dt
            pd + vd *dt
            vn + gnavx*dt + (dvx-dvx_b)*(q0^2 + q1^2 - q2^2 - q3^2)  - 2*(dvy-dvy_b)*(q0*q3-q1*q2) + 2*(dvz-dvz_b)*(q1*q3 + q0*q2)
            ve + gnavy*dt + (dvy-dvy_b)*(q0^2 - q1^2 + q2^2 - q3^2) + 2*(dvx-dvx_b)*(q0*q3+q1*q2) - 2*(dvz-dvz_b)*(q0*q1 - q2*q3)
            vd + gnavz*dt + (dvz-dvz_b)*(q0^2 - q1^2 - q2^2 + q3^2)  - 2*(dvx-dvx_b)*(q0*q2-q1*q3) + 2*(dvy-dvy_b)*(q0*q1 + q2*q3)
            dax_b
            day_b
            daz_b
            dvx_b
            dvy_b
            dvz_b
            magNavX
            magNavY
            magNavZ
            magX
            magY
            magZ
            ];
        
        qinit = quaternion(q0,q1,q2,q3);
        x(1:4) = compact(normalize(posangle(qinit * quaternion([dax, day, daz] - [dax_b, day_b, daz_b], 'rotvec'))));    
    
end

%STATETRANSJACOBIANFCN: Compute the Jacobian matrix dfdx of the state function of f(x)
%   x: state variable
%   dang: delta angle
%   dvel: deltavel
%   dt: sampling period
%   Approximation: cos(theta/2) = 1, sin(theta/2) = theta/2 
function dfdx = stateTransJacobianFcn(x, dang, dvel, dt)
        q0 = x(1);
        q1 = x(2);
        q2 = x(3);
        q3 = x(4);
        dax_b = x(11);
        day_b = x(12);
        daz_b = x(13);
        dvx_b = x(14);
        dvy_b = x(15);
        dvz_b = x(16);

        dax = dang(1);
        day = dang(2);
        daz = dang(3);

        dvx = dvel(1);
        dvy = dvel(2);
        dvz = dvel(3);

        H1 = [1, 1/2*(dax_b - dax), 1/2*(day_b - day), 1/2*(daz_b - daz), 0, 0, 0, 0, 0, 0, 1/2*q1, 1/2*q2, 1/2*q3, 0, 0, 0, 0, 0, 0, 0, 0, 0];
        H2 = [1/2*(dax-dax_b), 1, 1/2*(daz - daz_b), 1/2*(day_b - day), 0, 0, 0, 0, 0, 0, -1/2*q0, 1/2*q3, -1/2*q2, 0, 0, 0, 0, 0, 0, 0, 0, 0];
        H3 = [1/2*(day-day_b), 1/2*(daz_b - daz), 1, 1/2*(dax - dax_b), 0, 0, 0, 0, 0, 0, -1/2*q3, -1/2*q0, 1/2*q1, 0, 0, 0, 0, 0, 0, 0, 0, 0];
        H4 = [1/2*(daz-daz_b), 1/2*(day - day_b), 1/2*(dax_b - dax), 1, 0, 0, 0, 0, 0, 0, 1/2*q2, -1/2*q1, -1/2*q0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
        
        H8 =   [2*q0* (dvx - dvx_b) - 2*q3*(dvy - dvy_b) + 2*q2*(dvz - dvz_b),   2*q1*(dvx - dvx_b) + 2*q2*(dvy - dvy_b) + 2*q3*(dvz - dvz_b),  2*q1*(dvy - dvy_b) - 2*q2*(dvx - dvx_b) + 2*q0*(dvz - dvz_b),    2*q1*(dvz - dvz_b) - 2*q0*(dvy - dvy_b) - 2*q3*(dvx - dvx_b),     0, 0, 0,  1,  0,   0,    0,     0,     0, -(q0^2 + q1^2 - q2^2 - q3^2),        -2*(q1*q2 - q0*q3),          -2*(q1*q3 + q0*q2),  0, 0, 0, 0, 0, 0];
        H9 =   [2*q0* (dvy - dvy_b) + 2* q3*(dvx - dvx_b) - 2*q1*(dvz - dvz_b),   2*q2*(dvx - dvx_b) - 2*q1*(dvy - dvy_b) - 2*q0*(dvz - dvz_b), 2*q2*(dvy - dvy_b) + 2*q1*(dvx - dvx_b) + 2*q3*(dvz - dvz_b),  2*q0*(dvx - dvx_b) - 2*q3*(dvy - dvy_b) + 2*q2*(dvz - dvz_b),   0, 0, 0, 0, 1, 0, 0, 0, 0, -2*(q1*q2 + q0*q3),               -(q0^2-q1^2 + q2^2-q3^2), -2*(q2*q3 - q0*q1),             0, 0, 0, 0, 0, 0 ];                   
        H10 = [2*2*q1*(dvy - dvy_b) - 2* q2*(dvx - dvx_b) + q0* (dvz - dvz_b) , 2*q3*(dvx - dvx_b) + 2*q0*(dvy - dvy_b) - 2*q1*( dvz-dvz_b),  2*q3*(dvy - dvy_b) - 2*q0*(dvx - dvx_b) - 2*q2*(dvz - dvz_b), 2*q1*(dvx - dvx_b) + 2*q2*(dvy - dvy_b) + 2*q3*(dvz - dvz_b), 0, 0, 0,  0,  0,  1,     0,     0,     0,         -2*(q1*q3 - q0*q2),           -2*(q2*q3 + q0*q1),  -(q0^2-q1^2 - q2^2+q3^2),    0, 0, 0, 0, 0, 0 ];        

        dfdx = [...
                                               H1
                                               H2
                                               H3
                                               H4
                                                             0,                                                            0,                                                            0,                                                            0, 1, 0, 0, dt,  0,  0,     0,     0,     0,                           0,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 1, 0,  0, dt,  0,     0,     0,     0,                           0,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 1,  0,  0, dt,     0,     0,     0,                           0,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                H8
                                                H9
                                                H10
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     1,     0,     0,                           0,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     1,     0,                           0,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     1,                           0,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           1,                           0,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           1,                           0, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           1, 0, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           0, 1, 0, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           0, 0, 1, 0, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           0, 0, 0, 1, 0, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           0, 0, 0, 0, 1, 0, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           0, 0, 0, 0, 0, 1, 0
                                                             0,                                                            0,                                                            0,                                                            0, 0, 0, 0,  0,  0,  0,     0,     0,     0,                           0,                           0,                           0, 0, 0, 0, 0, 0, 1];


end

%POSANGLE: convert  negative quaternion to positive angle
%   The negative rotation is the same as positive rotation.
%   For example, -270 degree rotation is the same as +90 degree.
function q = posangle(q)
    indx = parts(q) < 0;
    if any(indx(:))
        q(idx) = -q(indx);
    end
end

%PLOTCHART: plot 3 data
function plotChart(estimated, graphTitle, xLabel, yLabel, dt)
    dimTrue = size(true);
    dimEstimated = size(estimated);
    
    totalLen = dimEstimated(1);
    
    time = dt * linspace(0, (totalLen(1)-1), totalLen(1));
    
    figure();
    for i =1:3
        subplot(3,1,i);    
        plot(time, estimated(1:totalLen,i));
        title(graphTitle(i));
        xlabel(xLabel);
        ylabel(yLabel);
        grid on;
    end
end

%PLOTCOMPARECHART: plot true and estimated chart
function plotCompareChart(true, estimated, graphTitle, xLabel, yLabel, dt)
    dimTrue = size(true);
    dimEstimated = size(estimated);
    
    totalLen = dimEstimated(1);
    
    time = dt * linspace(0, (totalLen(1)-1), totalLen(1));
    
    figure();
    for i =1:3
        subplot(3,1,i);    
        plot(time, true(1:totalLen,i));
        hold on;    
        plot(time, estimated(1:totalLen,i));
        title(graphTitle(i));
        xlabel(xLabel);
        ylabel(yLabel);
        legend('True', 'IMU');         
        grid on;
    end
end

%ADDITIVEPROCNOISE: Calculate process noise using sensor's bias information 
%   Additive process noise is model uncertainty 
%   process noise = process model uncertainty
%
%   GyroscopeBiasNoise: process noise for delta angle bias (rad/sec)^2 
%   AccelerometerBiasNoise: process noise for delta velocity bias (m/sec^2)^2 
%   GeomagneticVectorNoise: process noise for geomagnetic vector (uT/sec)^2
%   MagnetometerBiasNoise: process noise for magnetometer bias (uT/sec)^2 
function Qs = additiveProcNoise(GyroscopeBiasNoise, AccelerometerBiasNoise, GeomagneticVectorNoise, MagnetometerBiasNoise, OtherAdditiveNoise,imuFs)
    scale = 1/2 * (1/imuFs)^2;
    axis3 = ones(1, 3);
    
    dAngBiasSigma = scale * GyroscopeBiasNoise .* axis3;
    dVelBiasSigma = scale * AccelerometerBiasNoise .* axis3;
    geomagneticVectorSigma = scale * GeomagneticVectorNoise .* axis3;
    magnetometerBiasSigma = scale * MagnetometerBiasNoise .* axis3;
    
    Qs =diag([OtherAdditiveNoise .* ones(1,10), dAngBiasSigma, dVelBiasSigma, geomagneticVectorSigma, magnetometerBiasSigma]);        
end

% PROCNOISECOV: Calculate measurment noise covariance
function w = procNoiseCov(gyroscopeNoise, accelerometerNoise, imuFs)
    scale = 1/2 * (1/imuFs)^2;
    axis3 = ones(1,3);
    w = zeros(1, 6);
    
    w(1:3) = scale * gyroscopeNoise .* axis3;
    w(4:6) = scale * accelerometerNoise .* axis3;    
end

function  dwdx = procNoiseJacobianFcn(x, w)    % dw/dx
    daxCov = w(1);
    dayCov = w(2);
    dazCov = w(3);
    dvxCov = w(4);
    dvyCov = w(5);
    dvzCov = w(6);


    q0 = x(1);
    q1 = x(2);
    q2 = x(3);
    q3 = x(4);


    dwdx = [ ...
    (daxCov*q1^2)/4 + (dayCov*q2^2)/4 + (dazCov*q3^2)/4, (dayCov*q2*q3)/4 - (daxCov*q0*q1)/4 - (dazCov*q2*q3)/4, (dazCov*q1*q3)/4 - (dayCov*q0*q2)/4 - (daxCov*q1*q3)/4, (daxCov*q1*q2)/4 - (dayCov*q1*q2)/4 - (dazCov*q0*q3)/4,    0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
    (dayCov*q2*q3)/4 - (daxCov*q0*q1)/4 - (dazCov*q2*q3)/4,    (daxCov*q0^2)/4 + (dazCov*q2^2)/4 + (dayCov*q3^2)/4, (daxCov*q0*q3)/4 - (dayCov*q0*q3)/4 - (dazCov*q1*q2)/4, (dazCov*q0*q2)/4 - (dayCov*q1*q3)/4 - (daxCov*q0*q2)/4, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
    (dazCov*q1*q3)/4 - (dayCov*q0*q2)/4 - (daxCov*q1*q3)/4, (daxCov*q0*q3)/4 - (dayCov*q0*q3)/4 - (dazCov*q1*q2)/4,    (dayCov*q0^2)/4 + (dazCov*q1^2)/4 + (daxCov*q3^2)/4, (dayCov*q0*q1)/4 - (daxCov*q2*q3)/4 - (dazCov*q0*q1)/4, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
    (daxCov*q1*q2)/4 - (dayCov*q1*q2)/4 - (dazCov*q0*q3)/4, (dazCov*q0*q2)/4 - (dayCov*q1*q3)/4 - (daxCov*q0*q2)/4, (dayCov*q0*q1)/4 - (daxCov*q2*q3)/4 - (dazCov*q0*q1)/4,    (dazCov*q0^2)/4 + (dayCov*q1^2)/4 + (daxCov*q2^2)/4, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                               dvyCov*(2*q0*q3 - 2*q1*q2)^2 + dvzCov*(2*q0*q2 + 2*q1*q3)^2 + dvxCov*(q0^2 + q1^2 - q2^2 - q3^2)^2, dvxCov*(2*q0*q3 + 2*q1*q2)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyCov*(2*q0*q3 - 2*q1*q2)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzCov*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3), dvzCov*(2*q0*q2 + 2*q1*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxCov*(2*q0*q2 - 2*q1*q3)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyCov*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0, dvxCov*(2*q0*q3 + 2*q1*q2)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyCov*(2*q0*q3 - 2*q1*q2)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzCov*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3),                                                               dvxCov*(2*q0*q3 + 2*q1*q2)^2 + dvzCov*(2*q0*q1 - 2*q2*q3)^2 + dvyCov*(q0^2 - q1^2 + q2^2 - q3^2)^2, dvyCov*(2*q0*q1 + 2*q2*q3)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzCov*(2*q0*q1 - 2*q2*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxCov*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0, dvzCov*(2*q0*q2 + 2*q1*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxCov*(2*q0*q2 - 2*q1*q3)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyCov*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2), dvyCov*(2*q0*q1 + 2*q2*q3)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzCov*(2*q0*q1 - 2*q2*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxCov*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2),                                                               dvxCov*(2*q0*q2 - 2*q1*q3)^2 + dvyCov*(2*q0*q1 + 2*q2*q3)^2 + dvzCov*(q0^2 - q1^2 - q2^2 + q3^2)^2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
                                                   0,                                                      0,                                                      0,                                                      0, 0, 0, 0,                                                                                                                                                                0,                                                                                                                                                                0,                                                                                                                                                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

end


% CORRECTEQN compute measurement update
function [x, P, innov, innoCov]=correctEqn(x, P, h, H, z, R)
        
    innoCov = H * P * (H.') + R;
    K = P * (H.') / innoCov;
    
    innov = z - h;
    x = x + K*(innov);
    P = P - K*H*(P);    
end

%REPAIRQUATERNION: compute normalized positive rotation quaternion   
function q = repairQuaternion(q)
    q = q ./ sqrt(sum(q.^2));
    
    q = quaternion(q);
    
    idx = parts(q) < 0;    
    if any(idx(:))
        q(idx) = -q(idx);
    end
end

%MEASFCNTGPS: compute GPS measurement Hx or f(x)
function z = measFcnGPS(x)
    z(1) = x(5);   % position north
    z(2) = x(6);   % position east
    z(3) = x(7);   % position down
    z(4) = x(8);   % velocity north
    z(5) = x(9);   % velocity east
    z(6) = x(10); % celocity down
    
    z = z';
end

% 
function dhdx = measJacobianFcnGPS
    dhdx = [ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
            0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
end

%MEASFCNTGPS: compute GPS measurement Hx or f(x)
function z = measFcnMag(x)
    
    q0 = x(1);
    q1 = x(2);
    q2 = x(3);
    q3 = x(4);
    
    magNavX = x(17);     % North geomagnetic vector 
    magNavY = x(18);     % East geomagnetic vector
    magNavZ = x(19);     % Down geomgnetic vector
    magBiasX = x(20);    % magnetometer bias error in body frame x
    magBiasY = x(21);    % magnetometer bias error in body frame y
    magBiasZ = x(22);    % magnetometer bias error in body frame z

    mx = magBiasX + magNavX * (q0^2 + q1^2 - q2^2 - q3^2) - 2*magNavZ*(q0*q2 - q1*q3) + 2*magNavY*(q0*q3 + q1* q2);
    my = magBiasY + magNavY * (q0^2 - q1^2 + q2^2 - q3^2) + 2*magNavZ*(q0*q1 + q2*q3) - 2*magNavX*(q0*q3 - q1* q2);
    mz = magBiasZ + magNavZ * (q0^2 - q1^2 - q2^2 + q3^2) - 2*magNavY*(q0*q1 - q2*q3) + 2*magNavX*(q0*q2 + q1* q3);

    z = [mx, my, mz].';
    
end

% MEASJACOBIANFNCMAG: Measurement Jacobian Function of Magnetometer
function dhdx = measJacobianFcnMag(x)
    dhdx = zeros(3,22);

    q0 = x(1);
    q1 = x(2);
    q2 = x(3);
    q3 = x(4);

    magNavX = x(17);     % North geomagnetic vector 
    magNavY = x(18);     % East geomagnetic vector
    magNavZ = x(19);     % Down geomgnetic vector

    H1 = [2*q0*magNavX - 2*magNavZ*q2 + 2*magNavY*q3, 2 * q1 * magNavX + 2 * magNavZ * q3 + 2 * magNavY * q2, -2 * magNavX * q2 - 2 * magNavZ * q0 + 2 *magNavY * q1,    -2 * magNavX * q3 + 2 * magNavZ * q1 + 2 *magNavY * q0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,  (q0^2 + q1^2-q2^2-q3^2), 2*(q0*q3 + q1*q2),              -2*(q0*q2 - q1*q3),                1, 0, 0 ];
    H2 = [2 * magNavY * q0 + 2*magNavZ*q1 - 2 * magNavX * q3,    2*magNavY*q1 + 2*magNavZ*q0 + 2*magNavX*q2,  2*magNavY*q2 + 2*magNavZ*q3 + 2*magNavX*q1,               -2*magNavY*q3 + 2*magNavZ*q2 - 2*magNavX*q0,             0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,   -2*(q0*q3-q1*q2),            (q0^2 - q1^2 + q2^2 - q3^2), 2*(q0*q1 + q2*q3),                0, 1, 0];
    H3 = [2*magNavZ*q0 - 2*magNavY*q1  + magNavX*q2,    -2*magNavZ*q1 - 2*magNavY*q0 + 2*magNavX*q3,            -2*magNavZ*q2 + 2*magNavY*q3 + 2*magNavX*q0,               2*magNavZ*q3 + 2*magNavY*q2 + 2*magNavX*q1,            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,    2*(q0*q2 + q1*q3),         -2*(q0*q1 - q2*q3),               (q0^2 - q1^2 - q2^2 + q3^2),  0, 0, 1];
       
    dhdx(1,:) = H1;        
    dhdx(2,:) = H2;
    dhdx(3,:) = H3;
end








