%% ================================================================
%  Kalman Filter Prediction @ 100 Hz
%  ================================================================

%% STATE VECTOR
% dX = [errLAT errLong errAlt | errVN errVE errVD | errPhi errTheta errPsi |errorsBIASES (3 acc and 43 gyro biases)]

[q, Vned, Pned]=sinsq(q,Vned,Pned, A, W, DT, Gln, EARTH_R0, BA, BG);

% Convert quaternion to Rotation Matrix
Cbn = quat2cbn(q);

% Dynamic Matrix calculation
F = calculateF(Vned,A,Pned,Cbn);

% Matrix exponential
Fk = expm(F*DT);
Gk = calculateG(Cbn);

% Predicted covariance estimate
P = Fk*P*Fk'+Gk*Q*Gk'*DT;


%% ================================================================
%  Kalman Filter Correction @ 1 Hz
%  ================================================================

if(GPS_AVAILABLE)
    ...

    H1   = calculateH1; % gps velocity measurement
    dzV  = Vned-Vgps;
    PHT1 = P*(H1');
    RV   = diag(R_in(4:6)*0.1);

    K1   = PHT1/(H1*PHT1+RV);
    dx   = K1*dzV;
    P    = P-PHT1*K1';

    % Apply corrections
    Pned = Pned + dx(1:3);
    Vned = Vned + dx(4:6);

    % Correct attitude
    nq  = norm(dx(7:9));
    cnq = cos(nq/2);
    snq = sin(nq/2);
    dq  = [cnq; -dx(7:9)/nq*snq];
    q   = quatmul(dq,q);

    % Correct Biases
    BA = BA + dx(10:12);
    BG = BG + dx(13:15);

    dx = zeros(plen,1);

    ...
end