From 684b41494eac760683fe74fd16a7336881ac4ad1 Mon Sep 17 00:00:00 2001 From: George Hines Date: Thu, 7 Apr 2016 15:13:19 -0700 Subject: [PATCH] adds estimator log viewer UI --- matlab/analysis/estimatorLogViewer.m | 131 +++++++++++++++++++++++++++ matlab/analysis/usageSamples.m | 2 +- 2 files changed, 132 insertions(+), 1 deletion(-) create mode 100644 matlab/analysis/estimatorLogViewer.m diff --git a/matlab/analysis/estimatorLogViewer.m b/matlab/analysis/estimatorLogViewer.m new file mode 100644 index 0000000000..7c28035582 --- /dev/null +++ b/matlab/analysis/estimatorLogViewer.m @@ -0,0 +1,131 @@ +function estimatorLogViewer(fname) +close all; +load(fname); + +stateFieldNames = [fieldnames(estimatorData.EST0);fieldnames(estimatorData.EST1)]; +stateFieldNames(strcmp('T',stateFieldNames) | strcmp('index',stateFieldNames) | ... + strcmp('nStat',stateFieldNames) | strcmp('fNaN',stateFieldNames) | ... + strcmp('fHealth',stateFieldNames) | strcmp('fTOut',stateFieldNames)) = []; + +innovFieldNamesT = [fieldnames(estimatorData.EST4);fieldnames(estimatorData.EST5)]; +innovFieldNamesT(strcmp('T',innovFieldNamesT) | strcmp('index',innovFieldNamesT)) = []; +innovFieldInds = cellfun(@(x)x(end)=='V',innovFieldNamesT); +innovFieldNames = innovFieldNamesT(~innovFieldInds); + + +% lay out static objects +f = figure('Units','normalized'); +set(f,'Position',[.25,.25,.5,.5]); + +tabs = uitabgroup(f); +statesTab = uitab(tabs,'Title','States'); +innovsTab = uitab(tabs,'Title','Innovations'); + +axesConfig = {'Units','Normalized','Position',[.05,.1,.65,.85],'NextPlot','add'}; +BGconfig = {'Units','Normalized','Position',[.75,.1,.2,.85]}; +TBconfig = {'Style','Checkbox','Units','Normalized','Position',[.75,0,.2,.1],'String','Show variance'}; + +statesAxes = axes(statesTab,axesConfig{:},'XLabel',text('string','Time (sec)')); +innovsAxes = axes(innovsTab,axesConfig{:},'XLabel',text('string','Time (sec)')); + +statesBG = uibuttongroup(statesTab,BGconfig{:}); +innovsBG = uibuttongroup(innovsTab,BGconfig{:}); + +statesVarToggle = uicontrol(statesTab,TBconfig{:}); +innovsVarToggle = uicontrol(innovsTab,TBconfig{:}); + +% lay out dynamic objects +stateButtons = zeros(numel(stateFieldNames),1); +for k = 1:numel(stateFieldNames) + stateButtons(k) = uicontrol(statesBG,'Style','Checkbox','Units','normalized',... + 'String',stateFieldNames{k},'Position',[0,1 - k/numel(stateFieldNames),1, 1/numel(stateFieldNames)],... + 'Callback',@(es,ed)toggleLine(es,ed,statesAxes,statesVarToggle,estimatorData.EST0,estimatorData.EST1,estimatorData.EST2,estimatorData.EST3)); +end +innovButtons = zeros(numel(innovFieldNames),1); +for k = 1:numel(innovFieldNames) + innovButtons(k) = uicontrol(innovsBG,'Style','Checkbox','Units','normalized',... + 'String',innovFieldNames{k},'Position',[0,1 - k/numel(innovFieldNames),1, 1/numel(innovFieldNames)],... + 'Callback',@(es,ed)toggleLine(es,ed,innovsAxes,innovsVarToggle,estimatorData.EST4,estimatorData.EST5,[],[])); +end + +set(statesVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,statesAxes,estimatorData.EST2,estimatorData.EST3)); +set(innovsVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,innovsAxes,estimatorData.EST4,estimatorData.EST5)); +end + +function toggleLine(es,~,axes,varToggle,struct1,struct2,vstruct1,vstruct2) +if es.Value == 0 + delete(findobj(axes,'UserData',es.String)); +else + if any(strcmp(es.String,fieldnames(struct1))) + dataSrc = struct1; + else + dataSrc = struct2; + end + p = plot(dataSrc.T,dataSrc.(es.String)); + set(p,'Parent',axes,'UserData',es.String); +end +if varToggle.Value == 1 + if ~isempty(vstruct1) && ~isempty(vstruct2) + toggleVariance(varToggle,[],axes,vstruct1,vstruct2); + else + toggleVariance(varToggle,[],axes,struct1,struct2); + end +end +updateLegend(axes); +end + +function toggleVariance(es,~,axes,struct1,struct2) +if es.Value == 0 + delete(findobj(axes,'Type','Patch')); +else + lines = findobj(axes,'Type','Line'); + if isempty(lines) + return; + end + stateNames = {lines.UserData}; + if any(strncmp('s',stateNames,1)) + varNames = strrep(stateNames,'s','P'); + else + varNames = strrep(stateNames,'I','IV'); + end + for k = 1:numel(varNames) + if any(strcmp(varNames{k},fieldnames(struct1))) + dataSrc = struct1; + else + dataSrc = struct2; + end + centerLineTimeFull = get(lines(k),'XData'); + centerLineDataFull = get(lines(k),'YData'); + + startTime = max(centerLineTimeFull(1),dataSrc.T(1)); + endTime = min(centerLineTimeFull(end),dataSrc.T(end)); + plotTimeFull = dataSrc.T(dataSrc.T >= startTime & dataSrc.T <= endTime); + plotDataFull = dataSrc.(varNames{k})(dataSrc.T >= startTime & dataSrc.T <= endTime); + + centerLineTime = centerLineTimeFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); + centerLineData = centerLineDataFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); + + plotDataT = sqrt(interp1(plotTimeFull,plotDataFull,centerLineTime)); + + plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0); + if strcmp('IV',varNames{k}(end-1:end)) + plotDataL = downsample(-plotDataT,round(numel(plotDataT)/350),0); + plotDataU = downsample(plotDataT,round(numel(plotDataT)/350),0); + else + plotDataL = downsample(centerLineData-plotDataT,round(numel(plotDataT)/350),0); + plotDataU = downsample(centerLineData+plotDataT,round(numel(plotDataT)/350),0); + end + p = patch(axes,[plotTime,fliplr(plotTime)],[plotDataL,fliplr(plotDataU)],lines(k).Color); + set(p,'EdgeColor','none','FaceAlpha',.3,'UserData',stateNames{k}); + end +end +end + +function updateLegend(axes) +lines = findobj(axes,'Type','Line'); +if isempty(lines) + legend(axes,'off'); +else + legend(axes,lines,{lines.UserData}); +end +end \ No newline at end of file diff --git a/matlab/analysis/usageSamples.m b/matlab/analysis/usageSamples.m index b67802029e..e80df9ca7c 100644 --- a/matlab/analysis/usageSamples.m +++ b/matlab/analysis/usageSamples.m @@ -7,4 +7,4 @@ wholeLog = importPX4log(fname,{}); attitudeData = importPX4log(fname,{'ATT'}); %% estimator messages -estimatorData = importPX4log(fname,{'EST0','EST1','EST2','EST3','EST4','EST5','EST6'}); \ No newline at end of file +estimatorData = importPX4log(fname,{'EST0','EST1','EST2','EST3','EST4','EST5','EST6'});