Fixes verifyTvlqrController for estimator-centric controllers. Note: …

…TV controllers still not supported.

CompareTrajectoriesToData.m

@@ -1,27 +1,25 @@
 %% load data
 
 clear
-%%
+%% setup loading variables
 
-% TODO: detect these?
-%parameters = {0.904, 0.000, -0.134, -0.049, 0 };
-
-date = '2015-10-06';
+date = '2015-10-10';
 name = 'field-test';
-log_number = '12';
+log_number = '06';
 stabilization_trajectory = 0;
 hostname = 'odroid-gps3';
 
-trajectory_library = 'traj-archive/oct6-from-data.mat';
+trajectory_library = 'traj-archive/oct8-from-data-obstacle-ready.mat';
 
 use_simulation = false;
 
-
+%% load trajectory library
 
 disp(['Loading ' trajectory_library '...']);
 load(trajectory_library);
+disp('done.');
 
-
+%% load log data
 
 dir = [date '-' name '/' hostname '/'];
 filename = ['lcmlog_' strrep(date, '-', '_') '_' log_number '.mat'];
@@ -53,8 +51,9 @@
   this_alt = est.pos.z(idx);
   this_alt_end = est.pos.z(idx_end);
   this_alt_mid = est.pos.z( round((idx-idx_end)/2) + idx );
+  this_alt_max = max(est.pos.z);
 
-  if (this_alt > 5 || this_alt_end > 5 || this_alt_mid > 7.5)
+  if (this_alt > 5 || this_alt_end > 5 || this_alt_mid > 7.5 || this_alt_max > 7.5)
     t_starts = [t_starts t_starts_unfiltered(i)];
     t_ends = [t_ends t_ends_unfiltered(i)];
   else
@@ -92,8 +91,8 @@
 end
 
 %% create plots for papers/talks
-tmin = 169.9;
-tmax = 171.2;
+tmin = 125.4;
+tmax = 126.2;
 
 for i = 1 : 5
   figure(i)
@@ -105,14 +104,18 @@
 
 %% save files
 
-name_str = 'knife-edge-xyz';
+name_str = '2015-10-08_10';
+disp(['Saving ' name_str '...']);
 
 SaveComparison([name_str '-x'], 1, 1);
 SaveComparison([name_str '-y'], 1, 2);
 SaveComparison([name_str '-z'], 1, 3);
 
 SaveComparison([name_str '-roll'], 2, 1);
 SaveComparison([name_str '-pitch'], 2, 2);
+SaveComparison([name_str '-yaw'], 2, 3);
 
 SaveComparison([name_str '-u-left'], 3, 1);
 SaveComparison([name_str '-u-right'], 3, 2);
+
+disp('done.');

FindTvlqrStereoPairs.m

@@ -0,0 +1,36 @@
+% Finds stereo and trajectory reports nearby each other
+
+% search through log time
+
+clear paired_times
+threshold = 1;
+counter = 1;
+for i = 1 : length(stereo.logtime)
+
+  this_t = stereo.logtime(i);
+
+  [val, idx] = min(abs(tvlqr.logtime - this_t));
+
+  if abs(val) < threshold
+
+    tvlqr_t = tvlqr.logtime(idx);
+
+    pair = struct();
+    pair.tvlqr_t = tvlqr_t;
+    pair.stereo_t = this_t;
+    pair.delta_t = tvlqr_t - this_t;
+
+    paired_times(counter) = pair;
+
+    counter = counter + 1;
+  end
+end
+
+
+val = 0;
+for i = 1 : length(paired_times)
+  val = val + paired_times(i).delta_t;
+end
+average_delay = val/length(paired_times);
+
+disp(['Average delay over ' num2str(length(paired_times)) ' is ' num2str(average_delay) ' sec or ' num2str(average_delay*1000) ' ms']);

PlotTrajectoryPart.m

@@ -143,44 +143,46 @@ function PlotTrajectoryPart(est, u, traj_t, traj_x, traj_u, title_str, plot_deri
 
   %% plot u
 
-  label = 'Control surface deflection (deg)';
+
   figure_num = figure_num + 1;
   figure(figure_num)
 
   subplot(3,1,1);
   plot(u.logtime, rad2deg(u.rad.elevonL), 'b-');
   hold on
-  plot(traj_t, rad2deg(traj_u(1,:)), 'b--');
+  plot(traj_t, rad2deg(traj_u(1,:)), 'r-');
   grid on
   xlabel('Time (s)');
+  label = 'Left control surface deflection (deg)';
   ylabel(label);
   title([ 'LEFT: ' label ': ' title_str]);
   set(gca, 'FontSize', title_font_size);
   legend('Actual', 'Planned')
 
   subplot(3,1,2);
-  plot(u.logtime, rad2deg(u.rad.elevonR), 'k-');
+  plot(u.logtime, rad2deg(u.rad.elevonR), 'b-');
   hold on
-  plot(traj_t, rad2deg(traj_u(2, :)), 'k--');
+  plot(traj_t, rad2deg(traj_u(2, :)), 'r-');
 
 
   grid on
   xlabel('Time (s)');
+  label = 'Right control surface deflection (deg)';
   ylabel(label);
   title([ 'RIGHT: ' label ': ' title_str]);
   set(gca, 'FontSize', title_font_size);
   set(gca, 'XLimMode', 'auto');
   set(gca, 'YLimMode', 'auto');
 
   subplot(3,1,3);
-  plot(u.logtime, u.rad.throttle, 'r-');
+  plot(u.logtime, u.rad.throttle, 'b-');
   hold on
-  plot(traj_t, traj_u(3, :), 'r--');
+  plot(traj_t, traj_u(3, :), 'r-');
 
 
   grid on
   xlabel('Time (s)');
-  ylabel(label);
+  label = 'Throttle';
   title([ 'THROTTLE: ' label ': ' title_str]);
   set(gca, 'FontSize', title_font_size);
   set(gca, 'XLimMode', 'auto');

PoseToStateEstimatorVector.m

@@ -0,0 +1,44 @@
+function state = PoseToStateEstimatorVector(est, index, substract_positions, Mz)
+  % This mirrors RealtimeUtils.cpp's PoseMsgToStateEstimatorVector
+
+  state = zeros(12,1);
+
+  pos(1) = est.pos.x(index) - substract_positions(1);
+  pos(2) = est.pos.y(index) - substract_positions(2);
+  pos(3) = est.pos.z(index) - substract_positions(3);
+
+  % // x, y, and z are always in global frame (add in any custom rotation)
+  pos_global = Mz * pos';
+
+  state(1) = pos_global(1);
+  state(2) = pos_global(2);
+  state(3) = pos_global(3);
+
+  % // roll, pitch, and yaw come from the quats in the message
+
+  q(1) = est.orientation.q0(index);
+  q(2) = est.orientation.q1(index);
+  q(3) = est.orientation.q2(index);
+  q(4) = est.orientation.q3(index);
+
+  rot_mat = quat2rotmat(q');
+
+  rpy = rotmat2rpy(Mz * rot_mat);
+
+  state(4) = rpy(1);
+  state(5) = rpy(2);
+  state(6) = rpy(3);
+
+  % // velocities are in body frame
+  state(7) = est.vel.x(index);
+  state(8) = est.vel.y(index);
+  state(9) = est.vel.z(index);
+
+  % // rotation rates are given in body frame
+  %  // as angular velocities
+
+  state(10) = est.rotation_rate.x(index);
+  state(11) = est.rotation_rate.y(index);
+  state(12) = est.rotation_rate.z(index);
+
+end

ReadSimpleConfigServos.m

@@ -1,5 +1,6 @@
 function [rad_to_servo, servo_to_rad, minmaxtrim] = ReadSimpleConfigServos(filename)
   % Reads a .cfg file for bot-param-server and looks for particular fields
+  disp(['Reading ' filename ' for config info']);
 
 
   rad_to_servo = struct();
@@ -48,7 +49,7 @@
     end
 
     value = ReadKeyLine(tline, 'elevR_y_intercept');
-    if ~isempty(value)
+   if ~isempty(value)
       if current_obj == 1
         rad_to_servo.elevR_y_intercept = value;
       else

SaveComparison.m

@@ -7,5 +7,5 @@ function SaveComparison(name_str, fig_num, subplot_num)
   set(plot_new, 'Position', get(0, 'DefaultAxesPosition'));
   title('')
   legend('Actual','Planned');
-  saveasAll(['figures/' name_str], 20);
+  saveasAll(['figures/' name_str], 17);
 end

TrajectoryLibrary.m

@@ -234,6 +234,9 @@ function playback(obj, traj_num_from_filename)
     end
 
     function traj = GetTrajectoryByNumber(obj, traj_num_from_filename)
+      if (traj_num_from_filename > 1000)
+        traj_num_from_filename = 12;
+      end
       index = TrajectoryLibrary.IndexFromTrajectoryNumber(traj_num_from_filename);
       traj = obj.trajectories{index};
     end