Differential Equation transfer to Matlab ODE45 - were is my error?

Question

For a few hours I ponder about a problem that I suppose is totally easy to solve. I just don't get. In an assignment for Coursera (Aerial Robotics) I asked to program a PD Controller using the ODE45 function from matlab. The goal is (only!) the vertical stabilisation of an object along the z-axis.

The differential euqation of second order for this is

u(t) = m*(zd'' + Kp*(zd-z)+Kv(zd'-z')+g)

here d stays for desired position, z alone stands for the actual position and ' or '' stands for the 1st o. 2nd derivative. And u is the resulting force need to keep or get the object where it should be.

Now I tried to reduce it to a system of euqations of first order as follows:

zd = z1

dz1/dt = z2

dz2/dt= u(t)/m - (Kp*(zd-z)+Kv(zd'-z')+g)

z=z3

dz3/dt=z4

u=z5

dz5/dt=z6

And I think I have 5 starting variables for zd(0), zd'(0)=z2(0), z(0)=z3(0), z'(0)=z4(0) and u(0). And the goal is to get u(t).

Now I tried this code which unfortunately always delivers an error:

z0 = [s_des(1), s_des(2), s(1), s(2), u];
tlim=[t, t+.01];
[t,z]=ode45(@flight, tlim, z0);

function dzdt=flight(t,z)
    dzdt_1 = z(2);
    dzdt_2 = z(5)/m+kv*z(4)-kv*z(2)-kp*z(1)+kp*z(3)-g;
    dzdt_3 = z(4);
    dzdt_5 = z(6);
    dzdt=[dzdt_1; dzdt_2; dzdt_3; dzdt_5];
end

Here the s_des, s variables contain the starting or actual values.

The error is:

Index exceeds matrix dimensions. Error in controller/flight (line 42) dzdt_2 = z(5)/m+kvz(4)-kvz(2)-kpz(1)+kpz(3)-g;

Can anyone help me. I tried less equations but most often I get the same error or an error as this: CONTROLLER/FLIGHT returns a vector of length 5, but the length of initial conditions vector is 6. The vector returned by CONTROLLER/FLIGHT and the initial conditions vector must have the same number of elements.

edit: if I change the central function as follows I don't get any errors but it does not works neither as my object just drops to the ground ;-)

[t,z]=ode45(@flight, tlim, z0);

function dzdt=flight(t,z)
    dzdt_1 = z(2);
    dzdt_2 = z(5)/m+kv*z(4)-kv*z(2)-kp*z(1)+kp*z(3)-g;
    dzdt_3 = z(4);

    dzdt=[dzdt_1; dzdt_2; dzdt_3; 0; 0];
end

Thanks.

It's unclear to me what zd and z are meant to represent? Maybe it would be clearer if you didn't use zd as a variable name when dz and z are also floating about — Wolfie
Yes. That might be a good idea. But to the question: everything with zd stands for the desired vertical position and everything without a 'd' stands for the current position OK? — Andreas K.
What is z(6)? Based on your initial conditions, it looks like you only have five states. The inputs and outputs of your integration function need to have the same dimension as the initial conditions. — horchler
z(6) was a fertile try of mine to incorporate u = z5 into the equation. — Andreas K.

Andreas K. Andreas K. · Accepted Answer · 2017-10-29T15:30:21

After thinking and trying for some time I came up with this solution:

tlim=[tinit, tinit+.01];

iniCond=[s_des(1)-s(1),s_des(2)-s(2), s_des(1), s_des(2)];

[tSol, eSol]=ode45(@control, tlim, iniCond);

u=kv*m*eSol(end, 2) + m*kp*eSol(end,1) + m*g 

   function derror = control(time, error)
        A=[0 1 0 0;-kp  -kv 0 0;0 0 0 1;-kp -kv 0 0];
        b=[0 0 0 u/m-g]';
        derror=A*error+b;
    end

And this did the job of simulating the vertical movement of a quadcopter.

Differential Equation transfer to Matlab ODE45 - were is my error?

1 Answers