FormationControlSimulation/SOURCE/formationControl2ndBuilder.m

function obj = formationControl2ndBuilder(edgeL,robStateLen,kp1,kp2,ki1,ki2,dScale,nNodeVref)
    addpath("./networks-toolbox");
    obj = struct();
    [obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
    obj.m = length(obj.d);
    obj.n = numNodes(edgeL);
    obj.nodeStateLength = robStateLen*obj.n;
    obj.stateLength = 3 * obj.nodeStateLength + obj.m;

    % build matrix sistem
    obj.A1 = @(st) [zeros(obj.nodeStateLength), eye(obj.nodeStateLength), zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m)];
    obj.A2 = @(st) [-kp2*obj.R(st(1:(obj.nodeStateLength)),obj.K)'*obj.R(st(1:(obj.nodeStateLength)),obj.K), -kp1*eye(obj.nodeStateLength), -eye(obj.nodeStateLength), -obj.R(st(1:(obj.nodeStateLength)),obj.K)'];
    obj.A3 = @(st) [zeros(obj.nodeStateLength), ki1*eye(obj.nodeStateLength), zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m)];
    obj.A4 = @(st) [ki2*obj.R(st(1:(obj.nodeStateLength)),obj.K), zeros(obj.m,obj.nodeStateLength), zeros(obj.m,obj.nodeStateLength), zeros(obj.m)];
    obj.A = @(st) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st)];
    obj.sizeA = size(obj.A(ones(obj.stateLength,1)));

    % build matrix input
    obj.B = @(st) [zeros(obj.nodeStateLength,obj.m); kp2*obj.R(st(1:(obj.nodeStateLength)),obj.K)'; zeros(obj.nodeStateLength,obj.m); -ki2*eye(obj.m,obj.m)]
    obj.sizeB = size(obj.B(ones(obj.stateLength,1)));

    %build state space
    obj.Kb = zeros(obj.n*3,1);
    obj.Kb(obj.n+nNodeVref) = 1;
    obj.Kb = [kron(obj.Kb,eye(robStateLen)); zeros(obj.m,robStateLen)];
    obj.d = obj.d*dScale;
    obj.ssDisc = @(st,vRef,h) (eye(obj.sizeA) + (obj.A(st)*h))*st + obj.B(st)*h*obj.d + obj.Kb*vRef;
    obj.ss = @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;

    printf("============== Formation Control ============ \n");
    printf("           Node : %i \n", obj.n);
    printf("           Edge : %i \n", obj.m);
    printf("Dimention State : %i \n", obj.stateLength);
    printf("    Dimention A : %i x %i \n", obj.sizeA);
    printf("    Dimention B : %i x %i \n", obj.sizeB);
    printf("============================================= \n");

endfunction