% Jerry Pratt
% April 2, 1999
% Window M2 sim data for cartoon so not so many images...

% Good numbers:

start = 1;  %1300   
stop = 600;   %1500

q_la_pitch = q_la_pitch(start:stop);
q_la_roll = q_la_roll(start:stop);
q_lk = q_lk(start:stop);
q_lh_yaw = q_lh_yaw(start:stop);
q_lh_pitch = q_lh_pitch(start:stop);
q_lh_roll = q_lh_roll(start:stop);

q_ra_pitch = q_ra_pitch(start:stop);
q_ra_roll = q_ra_roll(start:stop);
q_rk = q_rk(start:stop);
q_rh_yaw = q_rh_yaw(start:stop);
q_rh_pitch = q_rh_pitch(start:stop);
q_rh_roll = q_rh_roll(start:stop);

q_x = q_x(start:stop);
q_y = q_y(start:stop);
q_z = q_z(start:stop);
q_yaw = q_yaw(start:stop);
q_pitch = q_pitch(start:stop);
q_roll = q_roll(start:stop);

vel = vel(start:stop);
xd = xd(start:stop);
yd = yd(start:stop);

left_state = left_state(start:stop);
right_state = right_state(start:stop);

t = t(start:stop);

q_x = q_x - q_x(1);
t = t - t(1);

tau_la_pitch = tau_la_pitch(start:stop);
tau_la_roll = tau_la_roll(start:stop);
tau_lk = tau_lk(start:stop);
tau_lh_yaw = tau_lh_yaw(start:stop);
tau_lh_pitch = tau_lh_pitch(start:stop);
tau_lh_roll = tau_lh_roll(start:stop);

tau_ra_pitch = tau_ra_pitch(start:stop);
tau_ra_roll = tau_ra_roll(start:stop);
tau_rk = tau_rk(start:stop);
tau_rh_yaw = tau_rh_yaw(start:stop);
tau_rh_pitch = tau_rh_pitch(start:stop);
tau_rh_roll = tau_rh_roll(start:stop);

qd_la_pitch = qd_la_pitch(start:stop);
qd_la_roll = qd_la_roll(start:stop);
qd_lk = qd_lk(start:stop);
qd_lh_yaw = qd_lh_yaw(start:stop);
qd_lh_pitch = qd_lh_pitch(start:stop);
qd_lh_roll = qd_lh_roll(start:stop);

qd_ra_pitch = qd_ra_pitch(start:stop);
qd_ra_roll = qd_ra_roll(start:stop);
qd_rk = qd_rk(start:stop);
qd_rh_yaw = qd_rh_yaw(start:stop);
qd_rh_pitch = qd_rh_pitch(start:stop);
qd_rh_roll = qd_rh_roll(start:stop);


pow_lh_pitch = qd_lh_pitch .* tau_lh_pitch;
pow_lh_roll = qd_lh_pitch .* tau_lh_roll;
pow_lh_yaw = qd_lh_pitch .* tau_lh_yaw;
pow_lk = qd_lk .* tau_lk;
pow_la_pitch = qd_la_pitch .* tau_la_pitch;
pow_la_roll = qd_la_pitch .* tau_la_roll;


pow_rh_pitch = qd_rh_pitch .* tau_rh_pitch;
pow_rh_roll = qd_rh_pitch .* tau_rh_roll;
pow_rh_yaw = qd_rh_pitch .* tau_rh_yaw;
pow_rk = qd_rk .* tau_rk;
pow_ra_pitch = qd_ra_pitch .* tau_ra_pitch;
pow_ra_roll = qd_ra_pitch .* tau_ra_roll;
