diff --git a/matlab/LinearMPC.m b/matlab/LinearMPC.m
index 47f06ad..6d3b53f 100644
--- a/matlab/LinearMPC.m
+++ b/matlab/LinearMPC.m
@@ -87,9 +87,7 @@
             Hu = kron(eye(obj.N),obj.R);
 
             H = blkdiag(Hq,Hqn,Hu);
-            
             y = ref_traj(:);
-            
             fx = y'*blkdiag(kron(eye(obj.N),obj.Q),obj.Qn);
             fu = zeros(obj.N*obj.Nu,1);
             f = -[fx';fu];
@@ -147,7 +145,7 @@
 
                 xl = lb;
                 xu = ub;
-                [QP,xout,fval,exitflag,iter,lambda] = qpOASES(H,f,A,xl,xu,al,au,obj.qpParams.options);
+                [xout,fval,exitflag,iter,lambda] = qpOASES(H,f,A,xl,xu,al,au,obj.qpParams.options);
             end  
         end
         
diff --git a/matlab/drone_sim/droneDynamics.m b/matlab/drone_sim/droneDynamics.m
index ee77309..a5777f9 100644
--- a/matlab/drone_sim/droneDynamics.m
+++ b/matlab/drone_sim/droneDynamics.m
@@ -1,7 +1,21 @@
-function [outputArg1,outputArg2] = droneDynamics(inputArg1,inputArg2)
-%DRONEDYNAMICS Summary of this function goes here
-%   Detailed explanation goes here
-outputArg1 = inputArg1;
-outputArg2 = inputArg2;
+function dqdt = droneDynamics(t,q,u,params)
+%droneDynamics Drone dynamics function, includes low level attitude
+%controller
+
+% q = x, y, z, dx, dy, dz, roll, pitch
+x=q(1);y=q(2);z=q(3);
+dx=q(4);dy=q(5);dz=q(6);
+roll=q(7);pitch=q(8);
+
+% u = roll, pitch, thrust  (command, angles in world frame);
+roll_command=u(1);pitch_command=u(2);thrust_command=u(3);
+
+% Attitude controller here, update dpitch and droll
+
+R = rotz(0)*roty(pitch_command)*rotx(roll_command); % roll and pitch in world frame so yaw irrelevant
+acc = [0;0;-params.g] + R*[0;0;thrust_command/params.m];
+
+dqdt = [dx;dy;dz;acc;0;0];
+
 end
 
diff --git a/matlab/drone_sim/sim_driver.m b/matlab/drone_sim/sim_driver.m
index f7afeda..0c5815c 100644
--- a/matlab/drone_sim/sim_driver.m
+++ b/matlab/drone_sim/sim_driver.m
@@ -1,42 +1,112 @@
+addpath('..')
+addpath('../qpOASES/interfaces/matlab')
+
+vis_factor = 5; % Slows down animation by this factor, should be 1 if MPC solves in real time
 
 N_traj = 200;
 N = 20;
 dt = 0.05;
+dt_attitude = 0.01; % Attitude controller update rate
+
+% System parameters
+params.g = 9.81;
+params.m = 5;
+k_cmd = 1;
+tau = 0.01;
+
+% Weights on state deviation and control input
+Qx = diag([1000 1000 1000 1 1 1 10 10]);
+Qn = 10*Qx;
+Ru = diag([0.1 0.1 0.01 0]);
+
+% Bounds on states and controls
+xmin = [-inf;-inf;-inf;-inf;-inf;-inf; -pi/3;-pi/3];
+xmax = [inf; inf; inf;inf;inf;inf; pi/3; pi/3];
+umin = [-pi/2;-pi/2; 0.5*params.m*params.g;1];
+umax = [pi/2; pi/2; 3*params.m*params.g; 1];
+
+stateBounds = [xmin xmax];
+controlBounds = [umin umax];
+
+% Linearized dynamics
+Ad = [1 0 0 dt 0  0  0  dt^2*params.g/2;
+      0 1 0 0  dt 0 -dt^2*params.g/2  0;
+      0 0 1 0  0  dt 0  0;
+      0 0 0 1  0  0  0  dt*params.g;
+      0 0 0 0  1  0  -dt*params.g 0;
+      0 0 0 0  0  1  0  0;
+      0 0 0 0  0  0  1-dt/tau  0;
+      0 0 0 0  0  0  0  1-dt/tau];
+  
+Bd = [0 0 0 0;
+      0 0 0 0;
+      0 0 dt^2/(2*params.m) -dt^2*params.g/2;
+      0 0 0 0;
+      0 0 0 0;
+      0 0 dt/params.m -params.g*dt;
+      k_cmd*dt/tau 0  0  0;
+      0 k_cmd*dt/tau  0  0];
+
+% Setup MPC object
+mpc = LinearMPC(Ad,Bd,Qx,Qn,Ru,stateBounds,controlBounds,N,'Solver','qpoases');
 
 % Reference Trajectory Generation
 xref = cos(0.02*(0:N_traj-1));
 yref = sin(0.02*(0:N_traj-1));
-zref = 0.1*(0:N_traj-1);
+zref = 0.02*(0:N_traj-1);
+rollref = zeros(size(zref));
+pitchref = zeros(size(zref));
 
-dxref = [0, diff(xref)/dt];
+dxref = [0,diff(xref)/dt];
 dyref = [0, diff(yref)/dt];
 dzref = [0, diff(zref)/dt]; 
 
-refTraj = [xref;yref;zref;dxref;dyref;dzref];
-xCur = refTraj(:,1);
-
-nx = size(refTraj,1);
+refTraj = [xref;yref;zref;dxref;dyref;dzref;rollref;pitchref];
+qCur = refTraj(:,1);
+nx = length(qCur);
 
 % Simulate
 step = 1;
 figure
 while(step + N < N_traj)
     tic
-    
+    %step
     % Get ref trajectory for next N steps
-    mpcRef = refTraj(:,step:step+N-1);
+    mpcRef = refTraj(:,step:step+N);
     
-    % Collect MPC Control (roll,pitch,yaw,thrust commands)
-    %u = ...
+    % Collect MPC Control (roll,pitch,thrust commands, all in world frame)
+    [Qout,fval] = mpc.solve(qCur,mpcRef);
+    Nx = 8;
+    Nu = 4;
+    xend = Nx*(N+1);
+    xout = Qout(1:Nx:xend);
+    yout = Qout(2:Nx:xend);
+    zout = Qout(3:Nx:xend);
+    dxout = Qout(4:Nx:xend);
+    dyout = Qout(5:Nx:xend);
+    dzout = Qout(6:Nx:xend);
+    rollout = Qout(7:Nx:xend);
+    pitchout = Qout(8:Nx:xend);
+
+    ustart = xend+1;
+    u1out = Qout(ustart+0:Nu:end);
+    u2out = Qout(ustart+1:Nu:end);
+    u3out = Qout(ustart+2:Nu:end);
+    u4out = Qout(ustart+3:Nu:end);
+    
+    u = [u1out(1);u2out(1);u3out(1)];
     
     % Simulate nonlinear drone dynamic model
-    %xCur = ...
-        
+    t0 = (step-1)*dt;
+    tf = t0 + dt;
+    [~,qNext] = ode45(@(t,q) droneDynamics(t,q,u,params),t0:dt_attitude:tf,qCur);
+    qCur = qNext(end,:)';
+    
     % Update plots and sleep for real time animation
-    xCur = mpcRef(1:3,1);
+    %qCur = mpcRef(1:3,1);
     plot3(mpcRef(1,:),mpcRef(2,:),mpcRef(3,:),'b')
     hold on
-    plot3(xCur(1,:),xCur(2,:),xCur(3,:),'r*')
+    plot3(qCur(1),qCur(2),qCur(3),'r*')
     hold off
     title('Drone Trajectory')
     
@@ -46,7 +116,7 @@
     grid on
     
     step = step + 1;
-    tsleep = dt - toc;
+    tsleep = dt*vis_factor - toc;
     if (tsleep > 0)
         pause(tsleep)
     end