From f14a39c34b0ec30c1cee20bc67680470ea63b9ee Mon Sep 17 00:00:00 2001 From: jbmag Date: Thu, 11 Mar 2021 17:12:31 +0100 Subject: [PATCH 1/3] print title string with underscore without interpreting it as tex --- _development/Matlab/PluginViewer/plugins/GenericEdge.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/_development/Matlab/PluginViewer/plugins/GenericEdge.m b/_development/Matlab/PluginViewer/plugins/GenericEdge.m index c8cdbfe..56c19d9 100644 --- a/_development/Matlab/PluginViewer/plugins/GenericEdge.m +++ b/_development/Matlab/PluginViewer/plugins/GenericEdge.m @@ -29,7 +29,7 @@ function GenericEdge( area, globalConfig, pluginConfig ) yl = max(std(err)); - title([pluginConfig.sensorName ' res']); + title([pluginConfig.sensorName ' res'], 'interpreter', 'none'); xlim([0, edge(end,1)-edge(1,1)]) ylim([-3*yl-eps 3*yl+eps]); From c1e259be774dffbe5299a6e20423b065015e6f1b Mon Sep 17 00:00:00 2001 From: jbmag Date: Thu, 11 Mar 2021 17:13:40 +0100 Subject: [PATCH 2/3] apply quaternion product on several quaternion at a time --- _development/Matlab/quatlib/quatprod.m | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/_development/Matlab/quatlib/quatprod.m b/_development/Matlab/quatlib/quatprod.m index ee04668..676eacb 100644 --- a/_development/Matlab/quatlib/quatprod.m +++ b/_development/Matlab/quatlib/quatprod.m @@ -1,10 +1,12 @@ function ret = quatprod(q1, q2) ret = [ - q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4); - q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3); - q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2); - q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1); + q1(:,1).*q2(:,1) - q1(:,2).*q2(:,2) - q1(:,3).*q2(:,3) - q1(:,4).*q2(:,4), ... + q1(:,1).*q2(:,2) + q1(:,2).*q2(:,1) + q1(:,3).*q2(:,4) - q1(:,4).*q2(:,3), ... + q1(:,1).*q2(:,3) - q1(:,2).*q2(:,4) + q1(:,3).*q2(:,1) + q1(:,4).*q2(:,2), ... + q1(:,1).*q2(:,4) + q1(:,2).*q2(:,3) - q1(:,3).*q2(:,2) + q1(:,4).*q2(:,1) ]; +a=q1(:,1).*q2(:,1) - q1(:,2).*q2(:,2) - q1(:,3).*q2(:,3) - q1(:,4).*q2(:,4); +b=q1(:,1).*q2(:,2) + q1(:,2).*q2(:,1) + q1(:,3).*q2(:,4) - q1(:,4).*q2(:,3); end From 231317c018ca1629235aa053a11430f9a5ad38a5 Mon Sep 17 00:00:00 2001 From: jbmag Date: Thu, 11 Mar 2021 17:15:14 +0100 Subject: [PATCH 3/3] StateObserver plugin can optionally show state error in addition to state --- .../PluginViewer/plugins/StateObserver.m | 120 +++++++++++++----- 1 file changed, 90 insertions(+), 30 deletions(-) diff --git a/_development/Matlab/PluginViewer/plugins/StateObserver.m b/_development/Matlab/PluginViewer/plugins/StateObserver.m index 916725e..6184282 100644 --- a/_development/Matlab/PluginViewer/plugins/StateObserver.m +++ b/_development/Matlab/PluginViewer/plugins/StateObserver.m @@ -1,37 +1,97 @@ function StateObserver(area, globalConfig, pluginConfig) -edgef = sprintf('%s%s.log',globalConfig.logPath, pluginConfig.sensorName); + edgef = sprintf('%s%s.log',globalConfig.logPath, pluginConfig.sensorName); -if exist(edgef, 'file') - - [edge, outcome] = stubbornLoad(edgef); - - if outcome == 1 + if exist(edgef, 'file') - subplot('Position', squeezeArea(area,0.02)) - - if strcmp(pluginConfig.stateComponent, 'x') - range = 4:6; - elseif strcmp(pluginConfig.stateComponent, 'q') - range = 7:10; - elseif strcmp(pluginConfig.stateComponent, 'v') - range = 11:13; - elseif strcmp(pluginConfig.stateComponent, 'w') - range = 14:16; - elseif strcmp(pluginConfig.stateComponent, 'a') - range = 17:19; - elseif strcmp(pluginConfig.stateComponent, 'alpha') - range = 20:22; - end - - plot(edge(:,1) - edge(1,1),[edge(:,range)]) - - axis tight - - title(sprintf('%s %s',pluginConfig.sensorName, pluginConfig.stateComponent)); - end -end - + [edge, outcome] = stubbornLoad(edgef); + + if outcome == 1 + isQuaternion = false; + + subplot('Position', squeezeArea(area,0.05)) + grid on + hold on + x_axis = 'Time [s]'; + plot_legend = {}; + if strcmp(pluginConfig.stateComponent, 'x') + range = 4:6; + state_name = 'Position'; + y_axis = 'Position [m]'; + plot_legend = {'x', 'y', 'z'}; + elseif strcmp(pluginConfig.stateComponent, 'q') + range = 7:10; + isQuaternion = true; + state_name = 'Orientation'; + y_axis = 'Orientation [rad]'; + plot_legend = {'roll', 'pitch', 'yaw'}; + elseif strcmp(pluginConfig.stateComponent, 'v') + range = 11:13; + state_name = 'Velocity'; + y_axis = 'Velocity [m.s^{-1}]'; + plot_legend = {'v_x', 'v_y', 'v_z'}; + elseif strcmp(pluginConfig.stateComponent, 'w') + range = 14:16; + state_name = 'Angular Velocity'; + y_axis = 'Angular Velocity [rad.s^{-1}]'; + plot_legend = {'{\omega}_x', '{\omega}_y', '{\omega}_z'}; + elseif strcmp(pluginConfig.stateComponent, 'a') + range = 17:19; + state_name = 'Acceleration'; + y_axis = 'Acceleration [m.s^{-2}]'; + plot_legend = {'a_x', 'a_y', 'a_z'}; + elseif strcmp(pluginConfig.stateComponent, 'alpha') + range = 20:22; + y_axis = ''; + end + + if (isfield(pluginConfig', 'referenceFile') && exist(pluginConfig.referenceFile, 'file')) + if strcmp(pluginConfig.stateComponent, 'x') + range_gt = 2:4; + elseif strcmp(pluginConfig.stateComponent, 'q') + range_gt = 5:8; + isQuaternion = true; + elseif strcmp(pluginConfig.stateComponent, 'v') + range_gt = 9:11; + elseif strcmp(pluginConfig.stateComponent, 'w') + range_gt = 12:14; + elseif strcmp(pluginConfig.stateComponent, 'a') + range_gt = 15:17; + elseif strcmp(pluginConfig.stateComponent, 'alpha') + range_gt = 18:20; + end + + [ground_truth, ~] = stubbornLoad(pluginConfig.referenceFile); + + % get values of groundtruth when at the same timesteps than the measurements + reference = interp1(ground_truth(:,1), ground_truth(:,range_gt), edge(:,1)); + + if isQuaternion + quat_rot_error = quatprod(quatinv(edge(:,range)), reference); + state_error = rad2deg(quat2euler(quat_rot_error)); + else + state_error = edge(:,range) - reference; + end + plot(edge(:,1) - edge(1,1), state_error) + title(sprintf('%s: error on estimated %s',pluginConfig.sensorName, state_name), 'interpreter', 'none'); + else + if isQuaternion + rpy_from_quat = rad2deg(quat2euler(edge(:,range))); + plot(edge(:,1) - edge(1,1),rpy_from_quat) + else + plot(edge(:,1) - edge(1,1),edge(:,range)) + end + title(sprintf('%s: estimated %s',pluginConfig.sensorName, state_name), 'interpreter', 'none'); + end + + axis tight + + xlabel(x_axis); + ylabel([y_axis]); + legend(plot_legend); + hold off + end + end end