%Assembling the matrix B
   function B=AssembleB(B,mseg,dseg,lseg,Iseg,AngleMatrix,k,n,eseg)
      B=zeros(n,n);
        for i=1:1:n
            for j=1:1:n
                %B is symmetrical so I just calculated half of that and sum
                %with the transposte
                if i==j
                    if i<n
                      B(i,j)= (mseg(i)*dseg(i)^2+mseg(i)*eseg(i)^2+sum(mseg(i+1:n))*lseg(i)^2+Iseg(i))/2;
                    else
                      B(i,j)= (mseg(i)*dseg(i)^2+mseg(i)*eseg(i)^2+Iseg(i))/2;
                    end
                elseif i>j
                 B(i,j)= (mseg(i)*dseg(i)+sum(mseg(i+1:n))*lseg(i))*lseg(j)*cos(AngleMatrix(k,j)-AngleMatrix(k,i))+mseg(i)*eseg(i)*lseg(j)*sin(-AngleMatrix(k,j)+AngleMatrix(k,i));
                end
            end
        end
        B=B+transpose(B);
  end


   %Assembling C
    function C=AssembleC(C,M,mseg,dseg,lseg,AngleMatrix,AngleVelMatrix,g,k,n,eseg)
      C=zeros(n,1);
        for i=1:1:n
            for j=1:1:n
                 if j<i
                  C(i)=C(i)+AngleVelMatrix(k,j)^2*((mseg(i)*dseg(i)+sum(mseg(i+1:n))*lseg(i))*lseg(j)*sin(AngleMatrix(k,j)-AngleMatrix(k,i))+mseg(i)*eseg(i)*lseg(j)*cos(-AngleMatrix(k,j)+AngleMatrix(k,i)));
                 elseif j>i
                  C(i)=C(i)+AngleVelMatrix(k,j)^2*(-(mseg(j)*dseg(j)+sum(mseg(j+1:n))*lseg(j))*lseg(i)*sin(AngleMatrix(k,i)-AngleMatrix(k,j))-mseg(j)*eseg(j)*lseg(i)*cos(AngleMatrix(k,j)-AngleMatrix(k,i)));
                 end
            end

                 if i<n
                  C(i)=C(i)+(mseg(i)*dseg(i)+sum(mseg(i+1:n))*lseg(i))*g*sin(AngleMatrix(k,i))+M(k,i+1)-M(k,i)-mseg(i)*eseg(i)*g*cos(AngleMatrix(k,i));
                 else
                  C(i)=C(i)+(mseg(i)*dseg(i)+sum(mseg(i+1:n))*lseg(i))*g*sin(AngleMatrix(k,i))-M(k,i)-mseg(i)*eseg(i)*g*cos(AngleMatrix(k,i));
                 end
        end
    end


% changing to a first order system
B'=[eye(n) zeros(n,n);zeros (n,n) B];
C'=[transpose(AngleVelMatrix(k,1:n));C];