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);