MATLAB: for loop changes sizing of matrix?

43 views Asked by At

I'm trying to linearize a state space model. Using the circshift function I'm trying to derive each element individually and add it all into one 12x12 matrix. In the for loop the code runs normally but then its as if the left side of the equation goes from an expected 4x3 to a 12x1.

clc 
clear

g = 9.8067;
m = 1.121;
cd = .23;
Jx = .01;
Jy = .0082;
Jz = .0148;
u=0;
UAV = 1;

JBx = (Jy - Jz)/Jx;
JBy = (Jz - Jx)/Jy;
JBz = (Jx - Jy)/Jz;

Un = [m*g, 0, 0, 0]';
Xn = [0 0 -1 0 0 0 0 0 0 0 0 0]';
delta = 0.001;

deltaX = delta*abs(Xn);
if (deltaX == 0) 
    deltaX = h;
end

deltaU = delta*abs(Un);
if (deltaU == 0) 
    deltaU = h;
end
temp = zeros(12,1);
temp(1) = 1;
Ax = zeros(12,12);
Bu = zeros(12,4);

test = QuadDynamics(Xn+circshift(temp,1)*deltaX(1),Un,UAV);
test2 = QuadDynamics(Xn,Un,UAV);
test3 = (test-test2)/deltaX(1);
%Runs correctly the first few times, then crashes because it tries to compute a 12x1 with a 4x4.
for i=1:12
    Ax(:,i) = (QuadDynamics(Xn+circshift(temp,i-1)*deltaX(i),Un,UAV) - QuadDynamics(Xn,Un,UAV))/deltaX(i);
end

for i=1:4
    Bu(:,i) = (QuadDynamics(Xn,Un+circshift(temp(1:4),i-1)*deltaU(i),UAV) - QuadDynamics(Xn,Un,UAV))/deltaU(i); 
end

A = Ax;
B = Bu;

%% Function xDot
function xDot = QuadDynamics(x,u,UAV)

    Velocity = [x(4);x(5);x(6)];
    EulerAngles = [x(7); x(8); x(9)];
    AngularRates = [x(10);x(11);x(12)];

    thrust = u(1);
    tphi = u(2);
    ttheta = u(3);
    tpsi = u(4);

    phi = EulerAngles(1);
    theta = EulerAngles(2);
    psi = EulerAngles(3);

    REBt = REB(phi, theta, psi);
    RBE = REBt';

    g = 9.8067;
    m = 1.121;
    cd = .23;
    Jx = .01;
    Jy = .0082;
    Jz = .0148;
    JBx = (Jy - Jz)/Jx;
    JBy = (Jz - Jx)/Jy;
    JBz = (Jx - Jy)/Jz;

    p = AngularRates(1);
    q = AngularRates(2);
    r = AngularRates(3);

    dPosition = Velocity';
    tempdVelocity = (1/m) * REBt * ([0; 0; -thrust] - RBE*Velocity*cd) + [0; 0; g];
    tempdEulerAngles = getN(phi, theta)*AngularRates;
    tempdAngularRates = [(JBx*q*r+(1/Jx)*tphi) (JBy*p*r + (1/Jy) * ttheta) (JBz*p*q + (1/Jz) * tpsi)];

    dVelocity = [tempdVelocity(1) tempdVelocity(2) tempdVelocity(3)];
    dEulerAngles = [tempdEulerAngles(1) tempdEulerAngles(2) tempdEulerAngles(3)];
    dAngularRates = [tempdAngularRates(1) tempdAngularRates(2) tempdAngularRates(3)];

    xDot = [dPosition; dVelocity; dEulerAngles; dAngularRates];
end

%% Function REB
function Rot = REB(phi,theta,psi)

    %Body to vehicle
    Rot = [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi)
           cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi)
           -sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta) ];
end

%% Function getN
function N = getN(phi, theta)

    N = [1 sin(phi)*tan(theta) cos(phi)*tan(theta)
         0 cos(phi) -sin(phi)
         0 sin(phi)*sec(theta) cos(phi)*sec(theta)];
end


I've tried splitting up the equation into multiple tests and all of them come back with expected results. I'm at a loss as to what is going wrong...

0

There are 0 answers