root/matlab/pmsm/mpf_u_delta_disp.m @ 230

%close all
itload('../../mpf_u_delta.it')
figure(1);
for i =1:4
        subplot(4,1,i)
    hold off
        plot(xth(:,i)')
end

figure(2)
for i=1:4
        subplot(4,1,i)
    hold off
        plot(xth(:,i)')
        hold on;
        plot(xthE(:,i)','r')
        plot(xthM(:,i+3)','g')
%       plot(xthV(i,:)','m');
    grid on
end

figure(7); plot(abs(xthM(:,3)));

figure(3)
tm=[1:9000]*125e-6;
subplot(2,1,1)
hold off
plot(tm,xthM(:,1),':')
hold on
plot(tm,xthM(:,2),'-')
set(gca,'XLim',[0,1.2]);
set(gca,'YLim',[-5,5]);
set(gca,'XTick',[]);
ylabel(['Estimated \Delta u [V]']);
legend('\Delta u_{\alpha s}', '\Delta u_{\beta s}')

subplot(2,1,2)
hold off
plot(tm,-Dt(:,3)+Dt(:,5),':')
hold on
plot(tm,-Dt(:,4)+Dt(:,6),'-')
set(gca,'XLim',[0,1.2]);
set(gca,'YLim',[-5,5]);
legend('\Delta u_{\alpha s}', '\Delta u_{\beta s}')
ylabel(['Simulated \Delta u [V]']);
xlabel(['time [s]']);
%       plot(Qtr(:,i)','--')


figure(4)
subplot(2,1,1)
hold off
di =xthE(:,1+2)'-xth(:,1+2)';
plot(tm,di,'--')
MSE_E=di*di'/9000

hold on;
di = xthM(:,3+3)'-xth(:,1+2)';
plot(tm,di,'-')
MSE_M=di*di'/9000
plot(tm, zeros(1,9000),':')
set(gca,'XLim',[0,1.2]);
title ('Estimate deviation')
ylabel('Rotor speed \omega')
legend('EKF', 'EKF with estimated voltage drops')

subplot(2,1,2)
hold off
di = xthE(:,2+2)'-xth(:,2+2)';
i2=find(di>1.5*pi);
di(i2)=di(i2)-2*pi;
i2=find(di<-1.5*pi);
di(i2)=di(i2)+2*pi;
plot(tm,di,'--')
MSE_E=di*di'/9000

hold on;
di =xthMTh' - xth(:,2+2)';
i2=find(di>1.5*pi);
di(i2)=di(i2)-2*pi;
i2=find(di<-1.5*pi);
di(i2)=di(i2)+2*pi;
plot(tm,di,'-')
MSE_M=di*di'/9000
plot(tm,zeros(size(tm)),':')
title ('Estimate deviation')
ylabel('Rotor position \vartheta')
set(gca,'XLim',[0,1.2]);
%set(gca,'YLim',[-0.5 0.5])
legend('EKF', 'EKF with estimated voltage drops')

% figure(3)
% ndat = size(ll,2);
% hold off
% plot(llgrid,ll(:,floor(ndat/3)),':');
% hold on
% plot(llgrid,ll(:,floor(2*ndat/3)),'--');
% plot(llgrid,ll(:,ndat));