Discrit time simulation for a LQR control doesn't follow reference

Hi!

I'm trying to make a simulation for a discret time LQR control, the model is an inverted pendulum. The simulation doesn't follow the referecenses. What to do?

Simulink model:

"Sistem discret" subsystem:

"Controller DtT1" subsystem:

Scope:

Matlab code:

clc 
clear all
close all
%% parametrii modelelului 2
Mp = 0.272;                     
dM = 0.071;                                              
g = 9.81;                       
J = 0.002;                       
Jw = 3.941*10^-5;              
b_theta = 0.82*10^-3;           
b_altha_kmke_Ra = 1.202*10^-4; 
km_Ra = 1.837*10^-4;

%% matricea modelului spatial
A = [0 1 0 0;
    (Mp*dM*g)/J -(b_theta)/J 0 1/J*b_altha_kmke_Ra;
    0 0 0 1;
    -(Mp*g*dM)/J b_theta/J 0 -(J+Jw)/(J*Jw)*b_altha_kmke_Ra];
B = [0 0;
    -1/J*km_Ra 1/J;
    0 0;
    (J+Jw)/(J*Jw)*km_Ra -1/J];
C = [1 0 0 0;
    0 1 0 0;
    0 0 1 0;
    0 0 0 1];
D = [0 0; 0 0;0 0;0 0];

%% construirea sistemului
sys = ss(A,B,C,0);
xd = [50;0;200;0]; %desired conditions 
x0 = 1*randn(4,1); %initial conditions 

%% Discretizare 
Ts = 0.1;
sys_d = c2d(sys,Ts);
Ad = sys_d.a;
Bd = sys_d.b;
Cd = sys_d.c;
Dd = sys_d.d;

Q = [100 0 0 0;
    0 100 0 0;
    0 0 10 0; 
    0 0 0 1];

Rd   = eye(size(B, 2));

K_d = dlqr(Ad,Bd,Q,Rd,N);

[Sahas]

Hi @Andrei Rotaru,

I replicated the Simulink model and get the exact graph that is provided.

There seems to be an issue in the MATLAB code, "N" in the last line is not defined.

Also what reference are you following as the matrix in the MATLAB code, "xd", seems to be different than the input provided to the serial bus "ref" input.

It would be helpful if you could provide the Simulink model as well as any revised MATLAB code for debugging purposes.