diff --git a/barrowaman.mat b/barrowaman.mat
deleted file mode 100644
index 4558b4a..0000000
Binary files a/barrowaman.mat and /dev/null differ
diff --git a/design/controller/lqr_algorithm.m b/design/controller/control_algorithm.m
similarity index 76%
rename from design/controller/lqr_algorithm.m
rename to design/controller/control_algorithm.m
index 7c081d3..bccf1e4 100644
--- a/design/controller/lqr_algorithm.m
+++ b/design/controller/control_algorithm.m
@@ -1,8 +1,14 @@
-function [u] = lqr_algorithm(x, r)
+function [u] = control_algorithm(x, r)
     % Computes control output. Uses gain schedule table and simplified roll model
     % Inputs: full state x, reference signal r
     % Outputs: control input u
     
+    persistent table; 
+
+    if isempty(table)
+        table = load("controller\gains.mat", "Ks", "P_mesh", "C_mesh");
+    end
+
     %% State
     % decompose state vector: [q(4); w(3); v(3); alt; Cl; delta]
     q = x(1:4); w = x(5:7); v = x(8:10); alt = x(11); Cl = x(12); delta = x(13);
@@ -16,11 +22,12 @@
     % cat roll state
     x_roll = [phi; w(1); delta];
 
-    %% Schedule
+    %% Gain scheduling
+    Ks = zeros(1,4);
     % get gain from schedule
-    Ks = lqr_schedule(x);
-    K_pre = Ks(4);
+    Ks = control_scheduler(table, x);
     K = Ks(1:3);
+    K_pre = Ks(4);
     
     %% Feedback law
     u = K*x_roll + K_pre*r; %two degree of freedom, full state feedback
diff --git a/design/controller/control_scheduler.m b/design/controller/control_scheduler.m
new file mode 100644
index 0000000..670eed2
--- /dev/null
+++ b/design/controller/control_scheduler.m
@@ -0,0 +1,37 @@
+function [K] = control_scheduler(table, x, dynamicpressure, canardcoeff)
+    % determines feedback gain from states. Interpolates from look-up table
+    % input: full state x
+    % output: K = [phi, p, delta, pre]
+
+    % check if state is provided
+    if isempty(x) == 0
+        % decompose state vector: [q(4); w(3); v(3); alt; Cl; delta]
+        v = x(8:10); alt = x(11); Cl = x(12);
+       
+        % calculate air data
+        [~, ~, rho, ~] = model_airdata(alt);
+        airspeed = norm(v);
+        p_dyn = rho/2*airspeed^2;
+    end 
+
+    % check for optinonal inputs
+    if nargin > 2 && isempty(dynamicpressure) == 0
+        p_dyn = dynamicpressure;
+    end
+    if nargin > 2 && isempty(canardcoeff) == 0
+       Cl = canardcoeff; 
+    end
+    
+    %% Load table
+    Ks = table.Ks;
+    P_mesh = table.P_mesh;
+    C_mesh = table.C_mesh;
+
+    %% Interpolate table
+    K = zeros(1,4);
+    for i=1:4
+        %%% interpolate linearly between design points, output 0 if state outside of table
+        K(i) = interp2(P_mesh, C_mesh, Ks(:,:,i), Cl, p_dyn, 'linear', 0); 
+    end
+end
+
diff --git a/design/controller/controller_module.m b/design/controller/controller_module.m
index 86392d0..4708ebc 100644
--- a/design/controller/controller_module.m
+++ b/design/controller/controller_module.m
@@ -1,7 +1,11 @@
-function [u] = controller_module(t, x)
+function [u] = controller_module(timestamp, x)
     % Top-level controller module. Calls controller algorithm. Sets reference signal.
     
-    % reference signal
+    %% stuff
+    t = timestamp;
+    r = 0;
+
+    %% reference signal
     %%% includes multiple roll angle steps. Reference r [rad].
     if t>10
         if t<15
@@ -11,11 +15,9 @@
         elseif t>25
             r = 0;
         end
-    else
-        r = 0;
     end
 
-    % compute controller output
-    u = lqr_algorithm(x, r);
+    %% compute controller output
+    u = control_algorithm(x, r);
 end
 
diff --git a/design/controller/gains.mat b/design/controller/gains.mat
index b7bce7f..864a8cb 100644
Binary files a/design/controller/gains.mat and b/design/controller/gains.mat differ
diff --git a/design/controller/lqr_schedule.m b/design/controller/lqr_schedule.m
deleted file mode 100644
index 8c3f42e..0000000
--- a/design/controller/lqr_schedule.m
+++ /dev/null
@@ -1,23 +0,0 @@
-function [K] = lqr_schedule(x)
-    % determines feedback gain from states. Interpolates from look-up table
-    % input: full state x
-    % output: K = [phi, p, delta, pre]
-
-    load("controller\gains.mat", "Ks", "P_mesh", "C_mesh");
-    %%% K = Ks(p_dyn, Cl, 1:3)
-    %%% K_pre = Ks(p_dyn, Cl, 4)
-
-    % decompose state vector: [q(4); w(3); v(3); alt; Cl; delta]
-    v = x(8:10); alt = x(11); Cl = x(12); 
-
-    % calculate air data
-    [~, ~, rho, ~] = model_airdata(alt);
-    airspeed = norm(v);
-    p_dyn = rho/2*airspeed^2;
-    
-    K = zeros(4,1);
-    for i=1:4
-        K(i) = interp2(P_mesh, C_mesh, Ks(:,:,i), Cl, p_dyn, 'linear'); 
-    end
-end
-
diff --git a/design/controller/lqr_tune_table.m b/design/controller/lqr_tune_table.m
index 28856c0..8c82f52 100644
--- a/design/controller/lqr_tune_table.m
+++ b/design/controller/lqr_tune_table.m
@@ -5,7 +5,7 @@
 
 % amount of design point for each dimension
 P_amount = 200; % dynamic pressure
-C_amount = 10; % coefficient of lift
+C_amount = 30; % coefficient of lift
 
 %% tuning parameters
 Q = diag([10, 0, 10]);
@@ -55,7 +55,7 @@
 
 %% Plot
 if 0
-    samplep = 1e5; samplec = 0;
+    samplep = 1e5; samplec = 1.5;
     for i=1:4
         K(i) = interp2(P_mesh, C_mesh, Ks(:,:,i), samplec, samplep, 'linear');
     end
@@ -70,6 +70,7 @@
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
     zlabel("K_\phi")
+    zlim([-1,1])
     
     % figure(2)
     subplot(2,2,2)
@@ -80,7 +81,8 @@
     hold off
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
-    zlabel("K_p")
+    zlabel("K_{\omega_x}")
+    zlim([-3,3])
     
     % figure(3)
     subplot(2,2,3)
@@ -92,6 +94,7 @@
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
     zlabel("K_\delta")
+    zlim([-4,0])
     
     % figure(4)
     subplot(2,2,4)
@@ -103,4 +106,5 @@
     xlabel("Coefficient")
     ylabel("Dynamic pressure")
     zlabel("K_{pre}")
+    zlim([-1,1])
 end
\ No newline at end of file
diff --git a/design/estimator/ekf_algorithm.m b/design/estimator/ekf_algorithm.m
index aa7477e..a68da0d 100644
--- a/design/estimator/ekf_algorithm.m
+++ b/design/estimator/ekf_algorithm.m
@@ -1,7 +1,7 @@
-function [x_new, P_new] = ekf_algorithm(x, P, u, y, t, Q, R, T, step)
+function [x_new, P_new] = ekf_algorithm(x, P, u, y, b, t, Q, R, T, step)
     % Computes EKF iteration. Uses model_f for prediction and model_h for correction.
     % use either this or ekf_predict and ekf_correct seperately
-    % Inputs: estimates x, P; control u; measurement y; timecode t
+    % Inputs: estimates x, P; control u; measurement y; sensor bias b; timecode t
     % Input parameters: weighting Q, R; time difference to last compute T; step size for ODE solver step
     % Outputs: new estimates x, P
 
@@ -12,18 +12,17 @@
     
     %%% solve IVP for x: x_dot = f(x, u)
     [x_new] = solver_vector(@model_f, T, step, t, x, u); % RK4
+    % x_new = x + T*model_f(t, x, u);
+    % x_new(1:4) = x_new(1:4)/norm(x_new(1:4)); % norm quaternions
 
     %%% compute Jacobian: F = df/dx
-    F = jacobian(@model_f, t, x, u, step); 
+    F = jacobian(@model_f, t, x, u); 
 
     %%% solve IVP for P: P_dot = F*P + P*F'+ Q
     %%% Heuns method
-    % P_dot = F*P + P*F'+ Q;
-    % P2 = P + T*P_dot;
-    % P_new = P + T/2*( P_dot + (F*P2 + P2*F'+ Q) ); 
-    %%% exact discretization method
-    A = F*T + eye(length(x));
-    P_pred = A*P*A' + Q;
+    P_dot = F*P + P*F'+ Q;
+    P2 = P + T*P_dot;
+    P_pred = P + T/2*( P_dot + (F*P2 + P2*F'+ Q) ); 
 
     %%% a-priori estimates
     x = x_new; P = P_pred;
@@ -34,29 +33,31 @@
     % Solves for covariance estimate 
 
     %%% compute expected measurement and difference to measured values
-    innovation = y - model_h(t,x,u);
+    innovation = y - model_h(t,x,b);
 
     %%% compute Jacobian: H = dh/dx
-    H = jacobian(@model_h, t, x, u, step); 
+    H = jacobian(@model_h, t, x, b); 
 
     %%% compute Kalman gain
-    S = H*P*H' + R;
-    K = P*H' * inv(S);
+    L = H*P*H' + R;
+    K = P*(H') * inv(L);
 
     %%% correct state and covariance estimates
     x_new = x + K*innovation;
-    x_new(1:4) = x_new(1:4)/norm(x_new(1:4)); % norm quaternions
+    % x_error = K*innovation;
+    % x_new = x + [zeros(4,1); x_error(5:7)];
+    % x_new(1:4) = x_new(1:4)/norm(x_new(1:4)); % norm quaternions
+
     % P_new = (eye(length(x)) - K*H ) * P; % standard form
     P_new = (eye(length(x))-K*H)*P*(eye(length(x))-K*H)' + K*R*K'; % joseph stabilized
 
     %% troubleshooting
-    P_pred = P_pred(1:11,1:11) %(1:11,1:11)
-    P_correct = P_new(1:11,1:11)%(1:13,1:11)
+    % P_pred = P_pred(1:11,1:11) %(1:11,1:11)
+    % P_correct = P_new(1:11,1:11)%(1:13,1:11)
     % Kalman = K%(1:11,:)
     % F_jac = F
     % H_jac = H
     % feedback_norm = norm(x_error(1:4))
     % quat_norm = norm(x(1:4))
     % rotmatrix = model_quaternion_rotmatrix(x(1:4));
-    t
 end
\ No newline at end of file
diff --git a/design/estimator/estimator_module.m b/design/estimator/estimator_module.m
index 261d727..31762d5 100644
--- a/design/estimator/estimator_module.m
+++ b/design/estimator/estimator_module.m
@@ -1,46 +1,64 @@
-function [xhat, Phat] = estimator_module(timestamp, omega, mag, accel, baro, cmd)
+function [xhat, Phat, bias, out] = estimator_module(timestamp, omega, mag, accel, baro, cmd)
     % Top-level estimator module. Calls EKF algorithm.
     % Inputs: concocted measurement and output vectors with multiple sensors. Not yet fully supported, work in progress
     % omega = [wS1; wS2; ...], mag = [magS1; magS2; ...], accel = [accelS1; accelS2; ...]
     % baro = [PS1; TS1; PS2; TS2; ...]
     % cmd = [CMDservo1; CMDservo2; ...]
     
-    persistent x P t; % remembers x, P, t from last iteration
-
+    persistent x P t b init_phase; % remembers x, P, t from last iteration
+    
     %% initialize at beginning
-    if isempty(P)
-        x = initializor([omega;mag;accel;baro]);
-        P = eye(length(x));
+    xhat = zeros(13,1); Phat = zeros(13); bias = zeros(6,1);
+    if isempty(x)
+        x = zeros(13,1);
+        P = zeros(length(x));
+        b = zeros(6,1);
+        init_phase = 1;
         if timestamp >= 0.005
-            t = timestamp-0.005;
+                t = timestamp-0.005;
         else 
-            t = 0;
+                t = 0;
+        end
+    end
+
+    if init_phase ~= 0 
+        [xhat, bias, ~] = initializor([omega;mag;accel;baro]);
+        x = xhat; b = bias;
+        if norm(accel) >= 12
+            init_phase = 0;
         end
+        xhat = x; Phat = P; bias = b;
+        % return
     end 
-    
+
     %% concoct y and u
     y = [omega; mag; baro];
     u = [cmd; accel];
 
     %% set parameters for EKF
-    step = 0.0025; % step size for RK4 and Jacobian
+    step = 0.0001; % step size for RK4 and Jacobian
     T = timestamp - t; % end time for RK4 and Improved Euler
     t = timestamp;
     
     %%% Q is a square 13 matrix, tuning for prediction E(noise)
     %%% x = [   q(4),           w(3),           v(3),      alt(1), Cl(1), delta(1)]
-    Q = diag([ones(1,4)*1e-5, ones(1,3)*1e1, ones(1,3)*1e-2, 1e-3,  0, 0]);
-    Q(1:4, 11) = 10;
-    % Q(1:4, 8:10) = 1;
+    Q = diag([ones(1,4)*3e-4, ones(1,3)*5e1, ones(1,3)*2e-1, 1e-2,  0, 0]);
+    % Q(1:4, 11) = 10;
     Q = (Q+Q')/2;
     
     %%% R is a square 7*a matrix (a amount of sensors), tuning for measurement E(noise)
     %%% y = [   W(3),          Mag(3),     P(1)]
-    R = diag([ones(1,3)*1e-4, ones(1,3)*1e-1, 1e2]);
+    R = diag([ones(1,3)*1e-1, ones(1,3)*5e0, 2e1]);
     R = (R+R')/2;
 
     %% compute new estimate with EKF
-    [xhat, Phat] = ekf_algorithm(x, P, u, y, t, Q, R, T, step);
-    x = xhat; P = Phat;
+    if init_phase == 0
+        [xhat, Phat] = ekf_algorithm(x, P, u, y, b, t, Q, R, T, step);
+        x = xhat; P = Phat;
+    end
+    
+    %% troubleshooting
+    timestamp;
+    out = 0;
 end
 
diff --git a/design/estimator/initializor.m b/design/estimator/initializor.m
index 7bcbc8b..720048f 100644
--- a/design/estimator/initializor.m
+++ b/design/estimator/initializor.m
@@ -1,43 +1,66 @@
-function [x_init] = initializor(meas)
+function [x_init, bias, out] = initializor(meas)
     % Computes inital state and covariance estimate for EKF, and bias values for the IMU
-    % Uses all available sensors: Gyroscope W, Magnetometer M,
-    % Accelerometer A, Barometer P and T, GPS lon, lat, alt
-    
+    % Uses all available sensors: Gyroscope W, Magnetometer M, Accelerometer A, Barometer P
+
+    persistent sensors; % remembers from last iteration
+
     %%% decompose measurement vector
     W = meas(1:3); M = meas(4:6); A = meas(7:9); P = meas(10);
-    
-    %%% load parameters
-    param = load("design/model/model_params.mat");
-
-    %%% compute quaternion attitude
-    %a = S_A*A;
-    %phi = atan(abs(a(3)/a(1))); % defines inital attitude on the rail, rail is pitched
-    phi = deg2rad(-5); % replace with attitude determination later on
-    d = [0;1;0];    
-    q = [cos(phi/2); d*sin(phi/2)];
 
-    %%% compute altitude
+    %%% load parameters
+    S_M = eye(3);
+    S_A = eye(3);
+    Cl_alpha = 1.5;
     g0 = 9.8; % zero height gravity
     air_R = 287.0579; % specific gas constant for air
     T_B = 288.15; % troposphere base temperature
     P_B = 101325; % troposphere base pressure
-    alt = -log(P/P_B)*air_R*T_B/g0;
 
-    %%% set inital values
+    %% Bias determination
+    y = meas(1:9);
+    if isempty(sensors)
+        sensors = y;
+    end
+    %%% lowpass to attenuate sensor noise
+    alpha = 0.05; % low pass time constant
+    sensors = sensors + alpha*(y-sensors); % lowpass filter
+
+    %% State determination
+    %%% gravity vector in body-fixed frame
+    a = S_A*sensors(7:9);
+
+    %%% compute launch attitude quaternion
+    psi = atan(-a(2)/a(1)); % rail yaw angle
+    theta = atan(a(3)/a(1)); % rail pitch angle
+    q = [cos(psi/2)*cos(theta/2); 
+         -sin(psi/2)*sin(theta/2);
+         cos(psi/2)*sin(theta/2);
+         sin(psi/2)*cos(theta/2)];
+
+    %%% compute altitude
+    alt = -log(P/P_B)*air_R*T_B/g0;
+    
+    %%% set constant initials
     w = [0; 0; 0];
     v = [0; 0; 0];
-    Cl = param.Cl_alpha;
+    Cl = Cl_alpha;
     delta = 0;
 
-    %%% compute earth magnetic field
-    S = model_quaternion_rotmatrix(q);
-    M_E = param.S_M*(S')*M;
-    
-    %%% save parameters
-    % save("design/estimator/initial_params.mat","M_E","-append")
-    
-    % q = [1; 0; 0; 0]; % initial attitude disturbance
     %%% conconct state vector
     x_init = [q;w;v;alt;Cl;delta];
+
+    %% Bias output
+    bias = zeros(6,1);
+    
+    %%% gyroscope
+    bias(1:3) = sensors(1:3);
+    
+    %%% compute earth magnetic field
+    S = model_quaternion_rotmatrix(q); % launch attitude
+    M_E = (S')*(S_M')*sensors(4:6); % sensorframe -> body-fixed -> earth-flat
+    bias(4:6) = M_E;
+
+    %% troubleshooting
+    out = [sensors; M_E; psi; theta];
 end
 
diff --git a/design/estimator/jacobian.m b/design/estimator/jacobian.m
index 8a475f9..57f5f17 100644
--- a/design/estimator/jacobian.m
+++ b/design/estimator/jacobian.m
@@ -1,9 +1,9 @@
-function [Jx] = jacobian(f, t, x, u, h)
+function [Jx] = jacobian(f, t, x, u)
     % Computes the Jacobian matrix of a vector function using complex-step differentiation
     % Inputs: ODE function f, time t, state x, step size h
     % Outputs: Jacobian matrix del_f/del_x
 
-    h = 0.0001; % step size overwrite
+    h = 0.001; % step size overwrite
 
     n = length(x); % size of input
     y = f(t, x, u);
diff --git a/design/estimator/solver_vector.m b/design/estimator/solver_vector.m
index c957478..e1aa155 100644
--- a/design/estimator/solver_vector.m
+++ b/design/estimator/solver_vector.m
@@ -1,6 +1,7 @@
 function [x_new] = solver_vector(f,T,step,t,x0,u)
     % Computes solution of Vector-valued IVP using an explicit Runge−Kutta method.
     % x_new at time t+T, using step size step
+    % RK4n method, with normalization of Quaternions x(1:4) in every time step
 
     % Explicit Runge-Kutta-4 coefficients
     a = [0, 1, 1, 2]/2; 
@@ -28,6 +29,7 @@
         end
         % value at time t+m*step
         x_m1 = x_m + step * (k*c');
+        x_m1(1:4) = x_m1(1:4)/norm(x_m1(1:4)); % quaternion normalization 
         %y(:, m+1) = x_m1;
         x_m = x_m1;
     end
diff --git a/design/model/model_f.m b/design/model/model_f.m
index 1000f4d..e81c549 100644
--- a/design/model/model_f.m
+++ b/design/model/model_f.m
@@ -16,9 +16,9 @@
             Jy = 52; % inertia pitch, yaw
             J = diag([Jx, Jy, Jy]);
             
-            length_cp = -1; % center of pressure
+            length_cp = -0.5; % center of pressure
             area_reference = pi*(8*0.0254/2)^2; % cross section of body tube
-            Cn_alpha = 1; % pitch coefficent 
+            Cn_alpha = 5; % pitch coefficent 
             
             %%% Sensors
             S_A = eye(3); % rotation transform from sensor frame to body frame
@@ -27,8 +27,8 @@
             %%% Canards, Actuator
             tau = 1/60; % time constant of first order actuator dynamics
             Cl_alpha = 1.5; % estimated coefficient of lift, const with Ma
-            tau_cl_alpha = 0.01; % time constant to converge Cl back to 1.5 in filter
-            area_canard = 0.005; % total canard area 
+            tau_cl_alpha = 2; % time constant to converge Cl back to 1.5 in filter
+            area_canard = 0.004; % total canard area 
             length_canard = 8/2*0.0254+0.05; % lever arm of canard to x-axis 
             c_canard = area_canard*length_canard; % moment arm * area of canard
             
@@ -80,7 +80,7 @@
     %%% acceleration specific force
     a = S_A*A - cross(w_dot, length_cs) - cross(w, cross(w, length_cs));
     %%% use aerodynamic for simulation, acceleration for filter
-    % v_dot = force - cross(w,v) + S'*g;
+    % v_dot = force - cross(w,v) + S*g;
     v_dot = a - cross(w,v) + (S)*g;
 
     % altitude derivative
diff --git a/design/model/model_h.m b/design/model/model_h.m
index 0851d9b..7b205d2 100644
--- a/design/model/model_h.m
+++ b/design/model/model_h.m
@@ -1,20 +1,23 @@
-function [y] = model_h(t, x, u)
+function [y] = model_h(t, x, b)
     % decompose state vector: [q(4); w(3); v(3); alt; Cl; delta]
     q = x(1:4); w = x(5:7); v = x(8:10); alt = x(11); Cl = x(12); delta = x(13);
 
+    % decompose bias vector: [b_W(3), M_E(3)]
+    b_W = b(1:3); M_E = b(4:6);
+
     % get parameters
     % z = load("design/estimator/initial_params.mat");
-    M_E = [49.77; -3.11; -18.76];
+    M_E = [-49.77; -3.11; 18.76];
     S_W = eye(3); % rotation transform from sensor frame to body frame
     S_M = eye(3); % rotation transform from sensor frame to body frame
 
     % rates
-    W = S_W*w; % rotation from sensor frame to rocket frame
+    W = S_W*(w - b_W); % rotation from sensor frame to rocket frame
 
     % magnetic field model
     % compute rotational matrix (attitude transformation matrix, between body frame and ground frame)
     S = model_quaternion_rotmatrix(q);
-    M = (S_M')*(S)*M_E; % M_E is initial orientation of magnetic field
+    M = (S_M)*(S)*M_E; % M_E is initial orientation of magnetic field
 
     % atmosphere model
     [P, ~, ~, ~] = model_airdata(alt);
diff --git a/design/model/model_params.m b/design/model/model_params.m
index 78e6b76..f7938e1 100644
--- a/design/model/model_params.m
+++ b/design/model/model_params.m
@@ -5,9 +5,9 @@
 J = diag([Jx, Jy, Jy]);
 
 length_cg = 0; % center of gravity
-length_cp = -1; % center of pressure
+length_cp = -0.5; % center of pressure
 area_reference = pi*(8*0.0254/2)^2; % cross section of body tube
-Cn_alpha = 1; % pitch coefficent 
+Cn_alpha = 5; % pitch coefficent 
 c_aero = Cn_alpha*area_reference*length_cp; % moment coefficient of body
 
 %% Sensors
@@ -20,7 +20,7 @@
 tau = 1/60; % time constant of first order actuator dynamics
 Cl_alpha = 1.5; % estimated coefficient of lift, const with Ma
 tau_cl_alpha = 0.01; % time constant to converge Cl back to 1.5 in filter
-area_canard = 0.005; % total canard area 
+area_canard = 0.002; % total canard area 
 length_canard = 8*0.0254+0.05; % lever arm of canard to x-axis 
 c_canard = area_canard*length_canard; % moment arm * area of canard
 
diff --git a/design/model/model_quaternion_rotmatrix.m b/design/model/model_quaternion_rotmatrix.m
index f40bee2..b4fb62f 100644
--- a/design/model/model_quaternion_rotmatrix.m
+++ b/design/model/model_quaternion_rotmatrix.m
@@ -1,14 +1,18 @@
 function [S] = model_quaternion_rotmatrix(q)
     % computes rotation matrix from quaternion
+    
+    %%% quaternion definition
+    q0 = q(1); qv = q(2:4);
 
-    % norm quaternions
+    %%% norm quaternions
     q = 1/norm(q) * q;
     
-    % skew symetric quaternion matrix
+    %%% skew symetric quaternion matrix
     q_tilde = [0, -q(4), q(3);
                q(4), 0, -q(2);
               -q(3), q(2), 0];
     
-    % rotation matrix
-    S = eye(3) + 2*q(1)*q_tilde + 2*q_tilde*q_tilde;
+    %%% rotation matrix
+    % S = (eye(3) + 2*q0*q_tilde + 2*q_tilde*q_tilde)'; Schiehlen
+    S = 2*qv*qv' + (q0^2-qv'*qv)*eye(3)-2*q0*q_tilde; % Stevens
 end
\ No newline at end of file
diff --git a/plant-model/CC_Flight_Simulation.slx b/plant-model/CC_Flight_Simulation.slx
index a469a36..c6f38b2 100644
Binary files a/plant-model/CC_Flight_Simulation.slx and b/plant-model/CC_Flight_Simulation.slx differ
diff --git a/plant-model/plant_model_components/Gravity_and_Propulsion.slx b/plant-model/plant_model_components/Gravity_and_Propulsion.slx
index 8b57c47..d629c07 100644
Binary files a/plant-model/plant_model_components/Gravity_and_Propulsion.slx and b/plant-model/plant_model_components/Gravity_and_Propulsion.slx differ
diff --git a/plant-model/plant_model_components/Sensors.slx b/plant-model/plant_model_components/Sensors.slx
index 85d5139..e95ce6d 100644
Binary files a/plant-model/plant_model_components/Sensors.slx and b/plant-model/plant_model_components/Sensors.slx differ
diff --git a/plant-model/plant_model_components/controller_subsystem.slx b/plant-model/plant_model_components/controller_subsystem.slx
index a1fa545..7e146d8 100644
Binary files a/plant-model/plant_model_components/controller_subsystem.slx and b/plant-model/plant_model_components/controller_subsystem.slx differ
diff --git a/plant-model/plant_model_components/estimator_subsystem.slx b/plant-model/plant_model_components/estimator_subsystem.slx
index ddd866c..0e1e8c7 100644
Binary files a/plant-model/plant_model_components/estimator_subsystem.slx and b/plant-model/plant_model_components/estimator_subsystem.slx differ
diff --git a/plant-model/plant_model_components/lift_compoenent.slx b/plant-model/plant_model_components/lift_compoenent.slx
index 93d1b7c..a23a9d9 100644
Binary files a/plant-model/plant_model_components/lift_compoenent.slx and b/plant-model/plant_model_components/lift_compoenent.slx differ
diff --git a/plant-model/plant_model_components/rocket_plant.slx b/plant-model/plant_model_components/rocket_plant.slx
index 274d87a..442f99d 100644
Binary files a/plant-model/plant_model_components/rocket_plant.slx and b/plant-model/plant_model_components/rocket_plant.slx differ
diff --git a/plant-model/plant_model_components/visualization.slx b/plant-model/plant_model_components/visualization.slx
index 932926a..d4dbfb1 100644
Binary files a/plant-model/plant_model_components/visualization.slx and b/plant-model/plant_model_components/visualization.slx differ
diff --git a/plant-model/scripts/borealis.m b/plant-model/scripts/borealis.m
index 05613ff..1e69cba 100644
--- a/plant-model/scripts/borealis.m
+++ b/plant-model/scripts/borealis.m
@@ -1,8 +1,14 @@
 %% OR Simulation Output Data
 or_data = readtable("plant-model\Data\Borealis\Borealis_flight_no_wind.csv");
 
+%% Initial values
+location = [10; 43.47; -80.54]; % launch location on earth. Altitude, Latitude, Longitude
+rail_angle = deg2rad(-5); % negative is pitched downrange
+rail_length = 8.28; % delta-altitude for rail constraints
+time_idle = 10; % wait time on the rail before launch
+
 %% Sensor parameters
-samplingrate = 0.01; % sampling period of the estimator
+samplingrate = 0.005; % sampling period of the estimator
 Ls1 = [-2.4;0;0]; % mounting location of IMU 1 relative CG
 Ss1 = eye(3); % mounting orientation of IMU 1 relative body frame
 
@@ -13,21 +19,16 @@
 act_anglelimit = 15; % max deflection in deg
 act_ratelimit = 600; % max rate in deg/s
 
-%% Initial values
-location = [10; 43.47; -80.54]; % launch location on earth. Altitude, Latitude, Longitude
-rail_angle = deg2rad(-5); % negative is pitched downrange
-rail_length = 8.28; % delta-altitude for rail constraints
-
 %% Reference Geometry
 %Reference parameters   
 Lr = 0.152; % reference length [m]
 Ar = pi * (Lr^2) / 4; % reference area [m^2]
 
 % Canards parameters 
-N_canard = 4;
-Cr_canard = 40 / 1000;
-Ct_canard = 40 / 1000; % "The tip is the size of the root to take advantage of the fact that the further away from the rocket, the greater the moment arm."
-span_canard = 80 / 1000;
+N_canard = 2;
+Cr_canard = 50 / 1000; %[m] root chord?
+Ct_canard = 30 / 1000; % %[m] tip chord?
+span_canard = 20 / 1000; %[m] height?
 arm_canard = 10/1000; % Moment arm from fin to fuselage
 alfa_canard = deg2rad(0); % Canard maximum angle of attack
 pos_canard = -(558.29 + 40)/1000;