You are an expert in writing code in many different languages. Your goal is to perform code completion for the following code, keeping in mind the rest of the code and the file meta data.
    Metadata details:
    filename: output_MPC_w_offset.m
    path of file: ./repos/quadrotor-pendulum
    
    the code of the file until where you have to start completion: %% QUADROTOR BALANCING PENDULUM MODEL PREDICTIVE CONTROL SIMULATION
%
% MATLAB simulation of the paper A Flying Inverted Pendulum by Markus Hehn 
% and Raffaello D'Andrea using a Model Predictive Controller
%
% Reference tracking with disturbance rejection using Optimal Target
% Selection (OTS) as in Lecture 6 page 17/21

%% INITIALIZATION
clc
clear
addpath('functions/');

disp('------------------------------------------------------------------');
disp('          OUTPUT MPC WITH DISTURBANCE REJECTION ');
disp('');
disp('------------------------------------------------------------------');


%% DEFINE CONSTANTS
g = 9.81;       % m/s^2
m = 0.5;        % kg
L = 0.565;      % meters (Length of pendulum to center of mass)
l = 0.17;       % meters (Quadrotor center to rotor center)
I_yy = 3.2e-3;  % kg m^2 (Quadrotor inertia around y-axis)
I_xx = I_yy;    % kg m^2 (Quadrotor inertia around x-axis)
I_zz = 5.5e-3;  % kg m^2 (Quadrotor inertia around z-axis)

%% DEFINE STATE SPACE SYSTEM
sysc = init_system_dynamics(g,m,L,l,I_xx,I_yy,I_zz);
check_controllability(sysc);

%% DISCRETIZE SYSTEM

    
function
%          N-1
% V(u_N) = Sum 1/2[ x(k)'Qx(k) + u(k)'Ru(k) ] + x(N)'Sx(N) 
%          k = 0

% tuning weights
Q = 10*eye(size(A));            % state cost
R = 0.1*eye(length(B(1,:)));    % input cost

% terminal cost = unconstrained optimal cost (Lec 5 pg 6)
[S,~,~] = dare(A,B,Q,R);        % terminal cost % OLD: S = 10*eye(size(A));

% prediction horizon
N = 10; 

Qbar = kron(Q,eye(N));
Rbar = kron(R,eye(N));
Sbar = S;

LTI.A = A;
LTI.B = B;
LTI.C = C;

dim.N = N;
dim.nx = size(A,1);
dim.nu = size(B,2);
dim.ny = size(C,1);

[P,Z,W] = predmodgen(LTI,dim);
              
H = (Z'*Qbar*Z + Rbar + 2*W'*Sbar*W);
d = (x0'*P'*Qbar*Z + 2*x0'*(A^N)'*Sbar*W)';
 
%% Define augmented observer

Bd = [0 0 1 0 0 0  0 0 0 0 0 0  0 0  0 0;
      0 0 0 0 0 0  0 0 1 0 0 0  0 0  0 0]';
Cd = [0   0   0    0   0   1    0    0  ;
      0   0   1    0   0   0    0    0  ]';
  
n_d = 2;

Aaug = [A           Bd;
        zeros(n_d,16) eye(n_d)];

Baug = [B; zeros(n_d,4)];

Caug = [C Cd];

test_Obs = [eye(16)-A -Bd; C Cd];
disp('Rank of Augmented System n+nd = 18 for full rank');
disp(rank(test_Obs))

Q_kf = 1*eye(8);
R_kf = 1*eye(16+n_d);

[~,Obs_eigvals,Obs_gain] = dare(Aaug',Caug',R_kf,Q_kf);
Obs_gain = Obs_gain';
disp('Eigenvalues of Kalman Filter Observer:');
disp(abs(Obs_eigvals))

%% Simulate system

u_limit = 0.1;

disp('------------------------------------------------------------------');
disp('                Simulating Output MPC System');
disp('------------------------------------------------------------------');
disp('');
fprintf('Simulation time: %d seconds\n',simTime);
disp('');

for k = 1:1:T
    t(k) = (k-1)*h;
    if ( mod(t(k),1) == 0 )
        fprintf('t = %d sec \n', t(k));
    end