MPC Controller for two wheeled robot - matlab implementation

Hi I'm trying to implement an MPC controller for a two wheeled robot. I took the formulars for it out of the paper "Model Predictive Control of a Mobile Robot Using Linearization" from Küne et al. In the following you see my code. I try to drive from x_start to x_goal But it does not do the right track. I don't really get where the fault is. Or how I should use the u_e I get from quadratic programming.

clc
clear all
close all
x_start = [0, 0, pi/2]';

x_goal = [0,1,0]
%#steps
steps = 10
v_r = 1/steps 
x_r_vec = zeros(3,10)
for i=1:steps
    x_r_vec(2,i) = i*v_r
end

x = zeros(3,steps);
dT = 0.1
theta_r = x_r_vec(3)%atan((x_goal(1)-x_now(1))/(x_goal(2)-x_now(2)));  
%x_start(3) = theta_r
x_now = x_start;
V1_0 = - v_r * sin(theta_r) * dT; % at time k
V2_0 = + v_r * cos(theta_r) * dT ;
A_0 = [1, 0, V1_0; 0, 1, V2_0; 0, 0, 1];
c_0 = cos(theta_r) * dT;
s_0 = sin(theta_r) * dT;
B_0 = [c_0, 0; s_0, 1; 0, dT];


for k=1:steps-1
    x(:,k) = x_now;
   
    theta_r_0 = x_r_vec(3,k)
    theta_r_1 = x_r_vec(3,k+1)
    V1_0 = - v_r * sin(theta_r_0) * dT; % at time k 
    V2_0 = + v_r * cos(theta_r_0) * dT ;
    c_0 = cos(theta_r_0) * dT;
    s_0 = sin(theta_r_0) * dT;    
    V1_1 = - v_r * sin(theta_r_1) * dT; % at time k + 1
    V2_1 = + v_r * cos(theta_r_1) * dT ;
    c_1 = cos(theta_r_1) * dT;
    s_1 = sin(theta_r_1) * dT;    
    
    A_1 = [1, 0, V1_1; 0, 1, V2_1; 0, 0, 1];
    
    B_1 = [c_1, 0; s_1, 1; 0, dT];
    Q = [1, 0, 0; 0, 1, 0; 0, 0, 0.5];
    R = [0.1, 0; 0, 0.1];
    Q_ = blkdiag(Q,Q);
    R_ = blkdiag(R, R);

    A_ =  [A_0;A_0*A_1];
    B_ = zeros(6,4);
    B_(1:3,1:2) = B_0;
    B_(4:6,3:4) = A_1*B_0;
    B_(4:6,3:4) = B_1;
   
    x_r = x_r_vec(:,k);%x_now + [cos(theta_r)*v_r; sin(theta_r)*v_r; theta_r];
    x_diff =  x_now - x_r;
    H = 2*(B_' * Q_ * B_ + R_);
    F = (2* x_diff' * A_' * Q_ * B_)';
    u_e = quadprog(H,F,[],[],[],[], [0,-0.4,0,-0.4],[2,0.4,2,0.4]); %x(:,i))
    u_e = u_e(1:2); % u_e = u - u_r
    w_r = 0;
    x_now = eye(3)*x_now + B_1*(u_e + [v_r;w_r]) ;
    A_0 = A_1;
    B_0 = B_1;
end
x(:,steps) =x_now;
figure
hold on 
plot(x(1,:),x(2,:))
plot(x_r(1,:),x_r(2,:))
plot(x_start(1),x_start(2),'x')
plot(x_goal(1),x_goal(2),'x')
legend('line','line_r','start','goal')