%Disturbance function: Generates random perturbations based on iteration within loop and specified
% - Params：
%   - size:Size of the model
%   - P：Number of disturbances
%   - k：Number of iterations
%   - I：Iteration upper bound
% - Return：
%   - R_p:Perturbation rotation matrix [3,3,P+1]
%   - T_p:Perturbation translation matrix [P+1,3]
function [ R_p, T_p ] = perturb( size,P,k,I )

%1.Initial translation disturbance:10% of the model size
T_p = normrnd(0,(0.1*size*((I+1-k)/I)),[P,3]); %[P,3]

%2.Initial perturbation number of rotation:10 ° normal distribution(Right hand coordinate system)
%Around x
a = normrnd(0,(10*((I+1-k)/I)),[1,P]); %[1,P]
%Around y
b = normrnd(0,(10*((I+1-k)/I)),[1,P]);
%Around z
c = normrnd(0,(10*((I+1-k)/I)),[1,P]);

%3.convert to radians
a = a.*(pi()/180);
b = b.*(pi()/180);
c = c.*(pi()/180);

%4.initialize R_p
R_p = zeros(3,3,P);

%5.Generate P disturbances respectively (matlab subscript starts from 1)
for j = 1:P
    %rotation matrix about x
    r1 = [1,0,0; 0, cos(a(j)), -sin(a(j)); 0, sin(a(j)), cos(a(j))]; 
    %rotation matrix about y
    r2 = [cos(b(j)), 0, sin(b(j)); 0,1,0; -sin(b(j)), 0, cos(b(j))];
    %rotation matrix about z
    r3 = [cos(c(j)), -sin(c(j)), 0; sin(c(j)), cos(c(j)), 0; 0,0,1];
    %Perturbation Rotation
    R_p(:,:,j) = r1*r2*r3;
end
%P+1:Transformation without disturbance
R_p(:,:,P+1) = eye(3);
T_p(P+1,:) = [0,0,0];
end

