diff --git a/src/mpc_examples.jl b/src/mpc_examples.jl
index e199d16..ac8ea48 100644
--- a/src/mpc_examples.jl
+++ b/src/mpc_examples.jl
@@ -449,39 +449,69 @@ function _build_mpc_example(::Val{:ballplate}, Np, Nc; settings=nothing, kwargs.
 end
 
 function _build_mpc_example(::Val{:crazyflie}, Np, Nc; settings=nothing, kwargs...)
-    F = [1.0 0.0 0.0 0.0000000 0.0009810 0.0000000 0.0100000 0.0000000 0.0000000 0.0000000 0.0000016 0.0000000;
-         0.0 1.0 0.0 -0.0009810 0.0000000 0.0000000 0.0000000 0.0100000 0.0000000 -0.0000016 0.0000000 0.0000000;
-         0.0 0.0 1.0 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0100000 0.0000000 0.0000000 0.0000000;
-         0.0 0.0 0.0 1.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0050000 0.0000000 0.0000000;
-         0.0 0.0 0.0 0.0000000 1.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0050000 0.0000000;
-         0.0 0.0 0.0 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0050000;
-         0.0 0.0 0.0 0.0000000 0.1962000 0.0000000 1.0000000 0.0000000 0.0000000 0.0000000 0.0004905 0.0000000;
-         0.0 0.0 0.0 -0.1962000 0.0000000 0.0000000 0.0000000 1.0000000 0.0000000 -0.0004905 0.0000000 0.0000000;
-         0.0 0.0 0.0 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000 0.0000000;
-         0.0 0.0 0.0 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000 0.0000000 0.0000000;
-         0.0 0.0 0.0 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000 0.0000000;
-         0.0 0.0 0.0 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 0.0000000 1.0000000]
-    G = [-0.0000011 0.0000012 0.0000011 -0.0000012;
-         0.0000011 0.0000012 -0.0000011 -0.0000012;
-         0.0002102 0.0002102 0.0002102 0.0002102;
-         -0.0068839 -0.0075809 0.0068916 0.0075732;
-         -0.0069177 0.0076070 0.0069392 -0.0076285;
-         0.0004937 -0.0001806 -0.0006961 0.0003830;
-         -0.0004524 0.0004975 0.0004538 -0.0004989;
-         0.0004502 0.0004958 -0.0004507 -0.0004953;
-         0.0420429 0.0420429 0.0420429 0.0420429;
-         -2.7535461 -3.0323404 2.7566264 3.0292601;
-         -2.7670702 3.0427842 2.7756950 -3.0514090;
-         0.1974771 -0.0722364 -0.2784376 0.1531969]
-
-    Q = 1 ./ ([0.1; 0.1; 0.1; 0.5; 0.5; 0.03; 0.5; 0.5; 0.5; 0.7; 0.7; 0.2].^2)
-    R = 1 ./ (([0.5; 0.5; 0.5; 0.5] / 6).^2)
-    Ts = 1e-2
-    u0 = [0.5833333520642209, 0.5833333520642209, 0.5833333520642209, 0.5833333520642209]
-
-    mpc = MPC(F, G; Ts, Np, Nc)
-    set_bounds!(mpc, umin = -u0, umax = ones(4) - u0)
-    set_objective!(mpc; Q, R, Rr = 0)
+    mass = get(kwargs, :mass, 0.035)
+    arm_length= get(kwargs, :arm_length, 0.046/1.414213562)
+    J = get(kwargs, :J,[1.66e-5 0.83e-6 0.72e-6; 
+                        0.83e-6 1.66e-5 1.8e-6; 
+                        0.72e-6 1.8e-6 2.93e-5])
+    thrustToTorque = get(kwargs,:thrustToTorque, 0.0008)
+    kt = get(kwargs,:kt, 2.245365e-6*65536) #thrust coefficient (PWM scale: 2^16-1)
+
+    diagonal_intertia = get(kwargs,:diagonal_intertia,true)
+    diagonal_intertia && (J=Diagonal(J))
+
+    g = 9.81
+    km = kt*thrustToTorque # moment coefficient
+
+    function cf_dynamics(x,u,d)
+        # auxiliary functions
+        hat(v) = [0 -v[3] v[2];v[3] 0 -v[1];-v[2] v[1] 0]
+        L(q) = [q[1] -q[2:4]'; q[2:4] q[1]*I+hat(q[2:4])]
+        T = cat(I(1),-I(3),dims=(1,2))
+        H = [zeros(1,3); I]
+        qtoQ(q) = H'*T*L(q)*T*L(q)*H
+
+        # Extract states
+        r = x[1:3]
+        q = x[4:6]
+        v = x[7:9]
+        ω = x[10:12]
+
+        qe = [1-q'*q;q] # true quaternion
+        Q = qtoQ(qe)
+
+        ṙ = v
+        q̇ = 0.5*L(qe)*H*ω
+
+        v̇ = [0; 0; -g] + (1/mass)*Q*[zeros(2,4); kt*ones(1,4)]*u 
+
+        Cu = [(arm_length*kt)*[-1 -1 1 1; -1 1 1 -1]; km*[-1 1 -1 1]];
+        ω̇ = J\(-hat(ω)*J*ω + Cu*u)
+
+        return [ṙ; q̇[2:4]; v̇; ω̇]
+    end
+
+    # Steady state when hovering  
+    x0 = zeros(12) 
+    u0 = (mass*g/kt/4)*ones(4)
+
+    # Start setting up MPC
+    Ts = get(kwargs,:Ts,1/500)
+    model = LinearMPC.Model(cf_dynamics,(x,u,d)->x,x0,u0,Ts)
+
+    mpc = LinearMPC.MPC(model;Np,Nc);
+    mpc.settings.reference_tracking = false;
+
+    Q =[156.25, 156.25, 400,  # position
+        2.777778, 2.777778, 1111.11111,  #angle
+        4,4,4, # velocity
+        4,4,25] # angular velocity
+    R = 50*[1,1,1,1]
+    set_objective!(mpc;Q=Q,R=R, Rr=0);
+
+    set_bounds!(mpc,umin = zeros(4), umax=ones(4));
+    set_terminal_cost!(mpc);
+    set_prestabilizing_feedback!(mpc);
 
     if isnothing(settings)
         mpc.settings.reference_tracking = false
@@ -490,8 +520,8 @@ function _build_mpc_example(::Val{:crazyflie}, Np, Nc; settings=nothing, kwargs.
     end
 
     range = ParameterRange(mpc)
-    range.xmax[:] .= 5
-    range.xmin[:] .= -5
+    range.xmax[:] .= 1
+    range.xmin[:] .=-1
 
     x0 = zeros(12)
     x0[4] = 0.1