% Model Parameters
    m = 1.244;                      % Quadrotor + battery mass
    g = 9.81;                       % Acceleration of gravity
%     Jxx = 0.0277;                   % Quadrotor and battery motor of inertia around bx (pitch)
%     Jyy = 0.0218;                   % Quadrotor and battery motor of inertia around by (roll)
%     Jzz = 0.0332;                   % Quadrotor and battery motor of inertia around bz (yaw)
    Jxx = 0.0130;                   % Quadrotor and battery motor of inertia around bx (pitch)
    Jyy = 0.0140;                   % Quadrotor and battery motor of inertia around by (roll)
    Jzz = 0.0285;                   % Quadrotor and battery motor of inertia around bz (yaw)
    Jreq = 4.2012e-05;              % Rotor and motor moment of inertia around axis of rotation 
    Kt = 8.6519e-6;                 % Rotor thrust constant
    Kh = 0;                         % Rotor in-plane drag constant
    Kd = 1.0317e-7;                 % Rotor drag constant
    rhx = 0.16;                     % X-axis distance from center of mass to a rotor hub
    rhy = 0.16;                     % Y-axis distance from center of mass to a rotor hub
    rhz = 0.03;                     % Z-axis distance from center of mass to a rotor hub
    Rm = 0.2308;                    % Motor resistance
    Kq = 96.3422;                   % Motor torque constant
    Kv = 96.3422;                   % Motor back emf constant
    If = 0.511;                     % Motor internal friction current
    Pmin = 1e5;                     % Minimum zybo output duty cycle command
    Pmax = 2e5;                     % Maximum zybo output duty cycle command
    Tc = 0.01;                      % Camera system sampling period
    tau_c = 0;                      % Camera system total latency
    Vb = 11.1;                      % Nominal battery voltage (V)
    x_controlled_o = 0;             % Equilibrium lateral controller output
    y_controlled_o = 0;             % Equilibrium longitudinal controller output
    yaw_controlled_o = 0;           % Equilibrium yaw controller output
    omega_o = sqrt((m*g)/(4*Kt));   % Equilibrium Rotor Speed
    
    % Equilibrium height controller output
    height_controlled_o = (((Rm*If + ...
    + (((omega_o * 2 * Rm * Kv * Kq  ...
    * Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
    *Kd))/Vb)*(Pmax- Pmin)+Pmin);