Upload New File

perturb.m

+%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
+