%close all
itload('../../mpf_u_delta_real.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+2)','g')
%	plot(xthV(i,:)','m');
    grid on
end


figure(3)
tm=[1:8000]*125e-6;
subplot(1,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',[]);
xlabel(['time [s]']);
ylabel(['Estimated \Delta u [V]']);
legend('\Delta u_{\alpha s}', '\Delta u_{\beta s}')

if 0
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)','--')
end

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

hold on;
di = xthM(:,1+4)'-xth(:,1+2)';
plot(tm,di,'-')
MSE_M=di*di'/8000
plot(tm, zeros(1,8000),':')
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'/8000

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'/8000
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));