systemID_project_kalman()

%Sumit Tripathi
function systemID_project_kalman()
clc
clear all;
close all;
format long;
load 'Turbinedata_sky.m';
Tdata=Turbinedata_sky(:,:);
%%DATA Sequence
%%time(m) time(d) time(yyyy) time(h) time(mm) time(ss)	rpm	powero	powerr
%%windspeed	watthrs
N1=1032;
N2=2926;
count=470;
w_l=3;
w_h=15;
w_filter1=w_l;
w_filter2=w_h;
[kwh wind]=minute_data(Tdata,count,N1,N2,w_filter1,w_filter2);% Extract Minute data
t_vec=linspace(1,length(wind),length(wind));
Hamm_win=0.54+0.46*cos(pi*(t_vec-max(t_vec)/2)/(max(t_vec)/2));% Apply Hamming Window to Input/ Output
Uwind=wind'.*Hamm_win;
Ukwh=kwh'.*Hamm_win;

L=length(Uwind);

y=kwh;
u=wind;
% % Marcov Parameters by FRF (Frequency Response Function)
dfftu=fft(u)/length(u);
dffty=fft(y)/length(y);
Syu=dffty.*conj(dfftu);
Suu=dfftu.*conj(dfftu);
G_yu_uu=Syu./Suu;
Y_mar=ifft(G_yu_uu);
Len=20;
H0=zeros(Len,1);% Initialization
H1=zeros(Len,1);
for itr=1:Len
    H0_col=0;
    H1_col=0;
    for t=1:Len
        H0_col=[H0_col;Y_mar(itr+t)];
        H1_col=[H1_col;Y_mar(itr+t+1)];
    end
    H0_col(1)=[];
    H1_col(1)=[];
    H0=[H0 H0_col];
    H1=[H1 H1_col];
    clear H0_col H1_col;
end
H0(:,1)=[]; % Clear first initialization column
H1(:,1)=[];
% Hankel Model order reduction technique
[R1,Sigma1,S1]=svd(H0*H0');

rolloff = diag(Sigma1)./max(max(Sigma1));
figure(1)
stem(rolloff);
title('Normalized Singular Values \sigma(i) of Hankel Matrix with DC');
ylabel('Normalized \sigma(i)')
order_n=4;
n=order_n;
[R,Sigma,S]=svd(H0);
P_alpha=R(:,1:n)*sqrtm(Sigma(1:n,1:n));
Q_beta=sqrtm(Sigma(1:n,1:n))*S(:,1:n)';
B_hat1=Q_beta(1:n,1)
C_hat1=P_alpha(1,1:n)
D=0;
rt_sigma_inv=inv(sqrtm(Sigma(1:n,1:n)));
A_hat1=rt_sigma_inv*R(:,1:n)'*H1*S(:,1:n)*rt_sigma_inv;
[V,E]=eig(A_hat1);
mag_E=(abs(diag(E)))
for i=1:length(mag_E)
    if mag_E(i)<1
        disp('Stable system')
    else
        disp('Unstable system')
    end
end
[Y,X]=lsim(A_hat1,B_hat1,C_hat1,D,wind,t_vec);

% keyboard
phiK=A_hat1;
Hk=C_hat1;
X_n=X';
dev=1e4;
dev2=1e-1;
Rk=.1;
Qk=diag([dev dev2*.1 dev2*.01 dev2*.001]);
Z=kwh;
Pk_bar=diag([dev dev dev dev]);
I=eye(length(Pk_bar));

X_bar=[];
    X_bar(:,1)=X_n(:,1);
    for k=1:length(X)
        
        %Step1
        Kk=Pk_bar*Hk'*inv(Hk*Pk_bar*Hk'+Rk);
        %Step2
        Xk_es=X_bar(:,k) + Kk*(Z(k)-Hk*X_bar(:,k));
        %Step3
        Pk=(I-Kk*Hk)*Pk_bar;
        X_bar(:,k+1)=phiK*Xk_es;
        Pk_bar=phiK*Pk*phiK'+Qk;
    end
X_bar(:,k)=[];
X_bar=X_bar';
ind=linspace(1,length(X),length(X));
%%Check coherence between identified states
Uwind=X_bar(:,1);
Ukwh=X_bar(:,2);
L=length(Ukwh);
del_t=1;
df=1/(L*del_t);
fmax=1/(2*del_t);
omega=0:df:fmax;
Y=Ukwh;
U=Uwind;
L=length(Ukwh)
del_t=1;
df=1/(L*del_t);
fmax=1/(2*del_t);
omega=0:df:fmax;
len=length(omega)
Cxy=mscohere(U,Y,len);
figure

plot(omega,Cxy(1:length(omega)));
axis manual
axis([0 max(omega) 0.6 1.01]);	% axis limits
grid on;
title('Coherence between Identified States With Kalman Filter');
xlabel('\omega')