%Kalman filter based on P2 and Q as measured variables. 

%% Kalman**

D1=D(:,1);
D2=D(:,2:3);
Bk=B(:,1);
Bdk=B(:,2:3);
sysk=ss(A,[Bk Bdk],C,[D1 D2],'inputname',{'Z','wG_in','wL_in'},'outputname',{'P1','P2','Q'},'statename',{'m_G1','m_L1','m_G2','m_L2'});
sensors =[2 3];
known=[1];
QN=0.02*[eye(2)]; % Standard deviation in measurements
RN=0.02*[eye(2)];  %Inputs uncertainty
NN=0;
[kest,L,P,M,Q] = kalman(sysk,QN,RN,NN,sensors,known)

%% ***LQG***
Qn=eye(4)*1; Rn=1*1000000; N=0;
%Qn=eye(4)*0.1; Rn=1*100000000; N=0;
[lqrk, S, e]=LQR(A,Bk,Qn,Rn,N)





