clear;
%Initialisering:
a1=0.0882;
b1=1.74;
a2=0.0882;
b2=1.838;
r=3;
A=r^2*pi;
u0=1.6;
u1=2.4;
u2=1.8;
uampl=.5;
w=.25;
Ku=15;
kv1=3;
kv2=2;

tslutt=60;
T=1;
t=[0:T:tslutt]';
N=length(t);
%xv10=(inn42(12)-b1)/a1;
%xv20=(inn42(11)-b2)/a2;
xv10=5;
xv20=5;
xeinit1=xv10;
xeinit2=xv20;
xeapo10=xeinit1;
xeapo20=xeinit2;
xeapr10=xeinit1;
xeapr20=xeinit2;
xv1=zeros(N,1)+xv10;
xv2=zeros(N,1)+xv20;
xm1=zeros(N,1);
xm2=zeros(N,1);
xeapo1=zeros(N,1)+xeapo10;
xeapo2=zeros(N,1)+xeapo20;
xeapr1=zeros(N,1)+xeapr10;
xeapr2=zeros(N,1)+xeapr20;
y=zeros(N,1);
yapr=zeros(N,1);
e=zeros(N,1);
u=zeros(N,1)+u1+uampl*sin(w*t);
Om=eye(2);
Q=[40 0;0 1];
R=0.01;
D=[0 1];
Papo=.1*eye(2);
Papr=.1*eye(2);
K=Papr*D'*inv(D*Papr*D'+R);

%Setter opp grafikk.    
figure(1);
clf;
title('x1 (heltrukken) og x1e (sirkler)')
ylabel('[cm]')
xlabel('Tid [sek]')
ha=gca;
set(ha,'GridLineStyle',':')     % Bruker . på grid,
set(ha,'NextPlot','add')        % Svarer til "hold on"
set(ha,'XGrid','on')            % Disse svarer til "Grid on"
set(ha,'YGrid','on')            % Kan bruke "grid on" i stedet....
set(ha,'XLim',[0 tslutt])            % Disse svarer til å sette aksene...
set(ha,'YLim',[0 25])            % Kan bruke "axis" i stedet....
lh1=line('color','y','EraseMode','none','xdata',[],'ydata',[]);
lh3=line('color','y','EraseMode','none','xdata',[],'ydata',[],...
'linestyle','-');
lh3a=line('color','y','EraseMode','none','xdata',[],'ydata',[],...
'linestyle','o');

figure(2);
clf;
title('x2 (heltrukken) og x2e (sirkler)')
ylabel('[cm]')
xlabel('Tid [sek]')
ha=gca;
set(ha,'GridLineStyle',':')     % Bruker . på grid,
set(ha,'NextPlot','add')        % Svarer til "hold on"
set(ha,'XGrid','on')            % Disse svarer til "Grid on"
set(ha,'YGrid','on')            % Kan bruke "grid on" i stedet....
set(ha,'XLim',[0 tslutt])            % Disse svarer til å sette aksene...
set(ha,'YLim',[0 25])            % Kan bruke "axis" i stedet....
lh2=line('color','m','EraseMode','none','xdata',[],'ydata',[]);
lh4=line('color','m','EraseMode','none','xdata',[],'ydata',[],...
'linestyle','-');
lh4a=line('color','m','EraseMode','none','xdata',[],'ydata',[],...
'linestyle','o');

%Her begynner simuleringen av prosess med Kalman-filter.

%Realiserer samplingsintervall paa T sek.:
tic
for k=2:N-1
   while toc<T  
   end; %while             
tic

%%%Simulering av virkelig prosess (prosess- og maalestoy simuleres ikke): 
xv1(k,1)=xv1(k-1,1)-(T*kv1/A)*sqrt(xv1(k-1,1))...
         +(T*Ku/A)*(u(k-1,1)-u0);
xv2(k,1)=xv2(k-1,1)+(T*kv1/A)*sqrt(xv1(k-1,1))...
         -(T*kv2/A)*sqrt(xv2(k-1,1));         
%xm1(k,1)=a1*xv1(k,1)+b1;
xm2(k,1)=a2*xv2(k,1)+b2;

%%%Paadrag settes ut til prosessen
%(Benyttes kun ved kjøring mot virkelig totank.)
%ut42(1,u(k,1))%Skriver ut spenning til IO-kortet

%%%Innlesning av maaling
%(Benyttes kun ved kjøring mot virkelig totank.)
%xm1(k,1)=inn42(12); %Leser inn måling fra IO-kortet
%xm2(k,1)=inn42(11); %Leser inn måling fra IO-kortet

%%%Invers maalefunksjon
%xv1(k,1)=(xm1(k,1)-b1)/a1;
xv2(k,1)=(xm2(k,1)-b2)/a2;
y=xv2(k,1);

%%%Innovasjonsprosessen
e(k,1)=y-yapr(k,1);

%%%Aposteriori-estimat for k
xeapr=[xeapr1(k,1);xeapr2(k,1)];
%K=K*0;
xeapo=xeapr+K*e(k,1);
xeapo1(k,1)=xeapo(1);
xeapo2(k,1)=xeapo(2);

%%%Apriori-estimat for k+1
xeapr1(k+1,1)=xeapo1(k,1)-(T*kv1/A)*sqrt(xeapo1(k,1))...
         +(T*Ku/A)*(u(k,1)-u0);
xeapr2(k+1,1)=xeapo2(k,1)+(T*kv1/A)*sqrt(xeapo1(k,1))...
         -(T*kv2/A)*sqrt(xeapo2(k,1));         

%%%Maaleestimat for k+1
yapr(k+1,1)=xeapr2(k+1,1);

%%%Beregning av Phi og D ved linearisering
Phi=[(1-T*kv1/(2*A*sqrt(xeapo1(k,1)))),0
(T*kv1/(2*A*sqrt(xeapo1(k,1)))),(1-T*kv2/(2*A*sqrt(xeapo2(k,1))))];
D=D;

%%%Kovarianser og K-forsterkning for k+1
Papr_1=Phi*Papo*Phi'+Om*Q*Om';
K_1=Papr_1*D'*inv(D*Papr_1*D'+R);
Papo_1=(eye(2)-K_1*D)*Papr_1*(eye(2)-K_1*D)'+K_1*R*K_1';
%%%For plotting
%K1(k+1)=K_1(1);K2(k+1)=K_1(2);

%%%Variabelskift fra k+1 til k
%home
K=K_1;
Papo=Papo_1;
Papr=Papr_1;

%For sanntids plotting på skjerm:
%Setter linjeobjekter
set(lh1,'xdata',[t(k-1,1)  t(k,1)],'ydata',[xv1(k-1,1) xv1(k,1)])        
set(lh2,'xdata',[t(k-1,1) t(k,1)],'ydata',[xv2(k-1,1) xv2(k,1)])
set(lh3,'xdata',[t(k-1,1)  t(k,1)],'ydata',[xeapo1(k-1,1) xeapo1(k,1)])        
set(lh3a,'xdata',[t(k-1,1)  t(k,1)],'ydata',[xeapo1(k-1,1) xeapo1(k,1)])        
set(lh4,'xdata',[t(k-1,1) t(k,1)],'ydata',[xeapo2(k-1,1) xeapo2(k,1)])
set(lh4a,'xdata',[t(k-1,1) t(k,1)],'ydata',[xeapo2(k-1,1) xeapo2(k,1)])
drawnow

end
disp('Simuleringen er ferdig')

%Paadraget settes tilbake til fast verdi
%(Benyttes kun ved kjøring mot virkelig totank.)
%ut42(1,u2) 
