R_Matrix.m

function [R] = R_Matrix(P_Triad, Euler_ang) %Functıon for R Matrix

r = Euler_ang(1);   %ROLL ANGLE from TRIAD at current step
p = Euler_ang(2);   %PITCH ANGLE from TRIAD at current step
y = Euler_ang(3);   %YAW ANGLE from TRIAD at current step

%Non-Linear Rotation Matrix From Euler to Quaternions
M_nl = [
    sin(r/2) * cos(p/2) * cos(y/2) - cos(r/2) * sin(p/2) * sin(y/2);...
    cos(r/2) * sin(p/2) * cos(y/2) + sin(r/2) * cos(p/2) * sin(y/2);...
    cos(r/2) * cos(p/2) * sin(y/2) + sin(r/2) * sin(p/2) * cos(y/2);...
    cos(r/2) * cos(p/2) * cos(y/2) + sin(r/2) * sin(p/2) * sin(y/2)...
    ];  

%LINEARIZED ROTATION MATRIX from EULER to QUATERNIONs
M = [(cos(p/2)*cos(r/2)*cos(y/2))/2 + (sin(p/2)*sin(r/2)*sin(y/2))/2, ...
    - (cos(p/2)*cos(r/2)*sin(y/2))/2 - (cos(y/2)*sin(p/2)*sin(r/2))/2, ...
    - (cos(r/2)*cos(y/2)*sin(p/2))/2 - (cos(p/2)*sin(r/2)*sin(y/2))/2; ...
    (cos(p/2)*cos(r/2)*sin(y/2))/2 - (cos(y/2)*sin(p/2)*sin(r/2))/2, ...
    (cos(p/2)*cos(r/2)*cos(y/2))/2 - (sin(p/2)*sin(r/2)*sin(y/2))/2, ...
    (cos(p/2)*cos(y/2)*sin(r/2))/2 - (cos(r/2)*sin(p/2)*sin(y/2))/2; ...
    (cos(r/2)*cos(y/2)*sin(p/2))/2 - (cos(p/2)*sin(r/2)*sin(y/2))/2, ...
    (cos(p/2)*cos(y/2)*sin(r/2))/2 - (cos(r/2)*sin(p/2)*sin(y/2))/2, ...
    (cos(p/2)*cos(r/2)*cos(y/2))/2 - (sin(p/2)*sin(r/2)*sin(y/2))/2; ...
    (cos(r/2)*sin(p/2)*sin(y/2))/2 - (cos(p/2)*cos(y/2)*sin(r/2))/2, ...
    (cos(p/2)*sin(r/2)*sin(y/2))/2 - (cos(r/2)*cos(y/2)*sin(p/2))/2, ...
    (cos(y/2)*sin(p/2)*sin(r/2))/2 - (cos(p/2)*cos(r/2)*sin(y/2))/2];

%TRANSFORMED and LINEARIZED COVARIANCE MATRIX of TRIAD
P_q = M * P_Triad * M';

%MEASUREMENT NOISE COVARIANCE MATRIX
R = [P_q zeros(4,3);
    zeros(3,4) diag([0.006 0.006 0.006])];
%RE CONSTRUCTION of R MATRIX
R = diag(R) .* eye(7,7);
end