Implement a Transfer Function in Code state space higher order

Hello,

I want to transform a transfer function to descrete time. I did this with the help of the matlab tutorial: https://www.youtube.com/watch?v=nkq4WkX7CFU&ab_channel=MATLAB

For this I have a six dimensional tf closedloop which can be seen below (instead of a 2D in the video)

When I give my tf a step response using the step function I get this desired result:

But when I run the code I get this, the output explodes:

The only thing I changed is the dimensions of x0 xd0 x and xd from 2 to 6, and used another transferfunction than the example.

Does anyone know what goes wrong here? I also tried the z domain method with the same result.

(here the original code can be found: https://github.com/aerojunkie/control-tools/blob/master/tfcodemethods.mlx)

Thank you very much!!

closedloop = (1.25e06*s^4 + 1.616e07*s^3 + 5.833e08*s^2 + 4.678e09*s + 7.234e09)/(s^6 + 469.9*s^5 + 1.276e06*s^4 + 1.639e07*s^3 + 5.939e08*s^2 + 4.688e09*s + 7.234e09)
step(closedloop)

%Make state space of it
statespace = ss(closedloop)

dt = 0.01;
t = 0:dt:14;

% Build step input
u = ones(size(t)); 
u(1) = 0; 

%tfcodemethods's code (only changed the dimesnsions from 2 --> 6)
% Initial conditions
x0 = [0; 0;0;0;0;0];
xd0 = [0; 0;0;0;0;0];

% Predefine the size of the state/ouput vectors
x = zeros(6,length(t));
xd = zeros(6, length(t));
y = zeros(size(t));

% Step through time and solve the differential equation
for i = 1:length(t)
    if i ==1
        x(:, i) = x0;
        xd(:,i) = statespace.A*x0 + statespace.B*u(i);
        y(i) = statespace.C*x0 + statespace.D*u(i);
    else
        x(:,i) = x(:,i-1) + dt*xd(:,i-1); % x = int(xd*dt)
        xd(:,i) = statespace.A*x(:,i) + statespace.B*u(i);  % State equation
        y(i) = statespace.C*x(:,i) + statespace.D*u(i);     % Output equation
    end
end
figure
scatter(t, y, 3);
legend('s-domain function', 'Differential equation approach',...
    'State space appraoch')

Answer 1

[Paul]

The short story is that dt is too large for this system. Try running with:

dt = 1e-5;
t = 0:dt:0.03;

And don't set u(1) = 0.

The original code used dt = 0.01. Looking at the step response shows that at t = 0.01 the response has already gone through at least one full oscillation by that time, so we know that trying to take one step from t = 0. to t = 0.01 is insufficient.

Answer 2

[Tobias]

@Paul thank you very much for your help.

Indeed when I change the dt value to e-5 it is plotted correctly.

But I will use this code on my arduino to control a DC motor, and there the max dt = 0.03.

Do you or maybe others know what I can do to fix this problem at larger dt?

This is the bode diagram of the closed loop system:

It is a mass spring damper system controlled with a PID and notch filter.

Maybe I have to lower the bandwidth or add another filter?

Thanks!