Commit 619f89ba authored by Velimir Todorovski's avatar Velimir Todorovski
Browse files

initial working version

parent bc5009d0
%% Simulation for fault identification of a linear time variant system of the form
% The fault and the disturbance are assumed to be described by a signal
% model of the form
% | x(t) | |A GP_d E_1P_f | | x(t) | | B |
%d/dt | v_d(t) | = |0 S_d 0 | | v_d(t) | + | 0 |u(t)
% | v_f(t) | |0 0 S_f | | v_f(t) | | 0 |
%
% | x(t) |
% y(t) = |C 0 E_2P_f | | v_d(t) |
% | v_f(t) |
%% Define the simulation time t_end, the interval length T and the discretization step dt
T = 2;
dt = 1e-2;
t_end = 3*T;
t =(-T:dt:t_end)';
tau = (0:dt:T);
%% Define System Dynamic
% the system matrices A(t), B(t), C(t), G(t), E_1(t), E_2(t)
% the matrices are defined as function handle matrices
A = @(t) [ -2 , 0 ;
0 , -4];
B = @(t) [1;
0];
C = @(t) [ -2 ,-3 ];
G = @(t) [ 0 ;
1 ];
E_1 = @(t) [ -7 ;
0 ];
E_2 = @(t) [0];
%% Define the fault signal model
% d/dt v_f(t) = S_f(t)v_f(t)
% f(t) = P_f(t)v_f(t)
S_f = @(t) 0;
P_f = @(t) 1;
%% Define the disturbance signal model
% d/dt v_d(t) = S_d(t)v_d(t)
% d(t) = P_d(t)v_d(t)
w0 = 2;
S_d = @(t) [0 , 1 ;
-(w0)^2 , 0];
P_d = @(t) [1 , 0 ];
%% define the extended System
n_x = size(A(1),1 );
n_u = size(B(1),2 );
n_y = size(C(1),1 );
n_d = size(G(1),2 );
n_f = size(E_1(1),2);
n_vd = size(S_d(1),1);
n_vf = size(S_f(1),1);
A_E = @(t) [A(t) , G(t)*P_d(t) , E_1(t)*P_f(t);
zeros(n_vd,n_x) , S_d(t) , zeros(n_vd,n_vf);
zeros(n_vf,n_x) , zeros(n_vf,n_vd), S_f(t)];
B_E = @(t) [B(t); zeros(n_vd,n_u); zeros(n_vf,n_u)];
C_E = @(t) [C(t), zeros(n_y,n_vd), E_2(t)*P_f(t)];
n_E = size(A_E(1),1);
%% Determining the Output
% here enter the intial values of the system
t1 =(0:dt:t_end);
opts = odeset('RelTol',1e-6,'AbsTol',1e-9);
fac = 70;
dt_dis = fac*dt;
t_dis = 0:dt_dis:t_end;
u = @(t) exp(-t).*sin(t);
%u = @(t) 0;
xE = ones(n_E,1);
[t_dis, xE ] = ode45(@(t,xE) A_E(t)*xE + B_E(t)*u(t), t_dis,[zeros(n_x,1);0;10;6], opts);
v_d = xE(:,(n_x+1):(n_vd+n_x)).';
v_f = xE(:,(n_vd+n_x+1):n_E).';
for k = 1:length(t_dis)
d(k,:) = P_d(t_dis(k)) * v_d(:,k);
y(k,:) = C_E(t_dis(k)) * xE(k,:).';
f(k,:) = P_f(t_dis(k)) * v_f(:,k);
end
% figure
% hold on
% plot(t_dis,f,'--');
% plot(t_dis,y,':','LineWidth',3);
% plot(t_dis,d);
% hold off
% t1 = t1.';
%% Discretization of the -A_E.' and C_E matrices for the whole simualtion interval [-T, t_end]
for k = 1:size(t,1)
Aek(k,:,:) = -A_E(t(k)).';
Ce_Glob(:,:,k)= C_E(t(k)).';
end
%% Simulating the Transition matrix for the whole simualtion interval [-T, t_end]
Phi = misc.fundamentalMatrix.odeSolver_par(Aek, t);
% n(tau,t) =
% C_E.'(tau,t)Phi.'(tau,tau_0,t)W^(-1)(tau_0,T,t)Phi(tau_0,T,t)m_E,i1
% m_E = Phi(tau,tau_0,t)int(Phi(tau_0,tau,t)C_E(tau,t)n(tau,t),tau,0,tau)
% with the controlabillity Gramian
% W(tau_0,T,t) =
% int(Phi(tau_0,tau,t)C_E(tau,t)C_E.'(tau,t)Phi.'(tau_0,tau,t),tau,0,T)
winlng = size(tau,2);
I = eye(n_vf);
N = zeros(winlng,size(t1,2),n_y,n_vf);
m_E = zeros(n_E,length(tau),length(t1),n_vf);
% The only a segment of the Phi matrix on the interval [t-T,t] is extracted
% and the calculation for m_E and n is done numerically for the interval [0, t_end]
for indtMinT = 1:size(t1,2)
indt = indtMinT + winlng -1;
Phitau = Phi(indtMinT:indt,indtMinT,:,:);
Ce_loc = Ce_Glob(:,:,indtMinT:indt);
%% Calculating W numerically
for k = 1:length(tau)
Phitt0(:,:,k) = Phitau(k,1,:,:);
Phit0t = inv(Phitt0(:,:,k));
w(k,:,:) = Phit0t*Ce_loc(:,:,k)*(Ce_loc(:,:,k).')*(Phit0t.');
end
tmp = reshape(w, length(tau), n_E^2);
tmp_I = trapz(tau, tmp);
W = reshape(tmp_I, n_E , n_E);
%% Calculating n denoted as N because its a matrix :)
PhiTt0(:,:,1) = Phitau(end,1,:,:);
Phit0T = inv(PhiTt0);
for k = 1:length(tau)
Phitt0(:,:,k) = Phitau(k,1,:,:);
Phit0t = inv(Phitt0(:,:,k));
for ki = 1:n_vf
N(k,indtMinT,:,ki) = (Ce_loc(:,:,k).')*(Phit0t.')*inv(W)*(Phit0T*[zeros(n_E-n_vf,1);I(:,ki)]);
end
% N(tau,t,n_y,r_sum)
end
%% calculating the states m_E
for ki = 1:n_vf
m_E(:,:,indtMinT,ki) = calculateStates(dt,tau,Phitau,Ce_loc,N(:,indtMinT,:,ki));
end
% m_E(nk,tau,t,r_sum)
end
%% calculating < m_E,B_Eu >
u_dis = u(t_dis);
u_foh = interp1(t_dis,u_dis,t1);
for ki = 1:n_vf
mB(:,ki) = kernTransformTwo(t,t1,tau,m_E(:,:,:,ki),B_E,u_foh);
end
%% calculating < n,y >
% y is assumed to be zero on the interval from [-T,0] and a segment of y is
% extracted on the interval from [t-T,t] and then multiplied with n for
% every t, the result is stored in NY
y_foh = interp1(t_dis,y,t1).';
for ind2 = 1:length(t1)
Summe = zeros(n_vf,1);
yk = zeros(winlng,n_y);
diff = winlng - ind2;
if((t1(ind2)-T) < 0)
ind1 = 1;
yk((ind1+diff):winlng,:) = y_foh(ind1:ind2,:);
elseif((t1(ind2)-T)>=0)
ind1 = ind2-winlng+1;
yk = y_foh(ind1:ind2,:);
end
for j = 1:n_y
for ki = 1:n_vf
Summe(ki,1) = trapz(tau,N(:,ind2,j,ki).*yk(:,j)) + Summe(ki,1);
end
end
NY(ind2,:) = Summe;
end
% NY(t,r_sum)
%% calculating ZOH
% delta_t = dt_dis;
% N_k = find(t_dis == T)-1;
%
%
% for t_ind = 1:length(t1)
% yk = zeros(winlng,n_y);
% diff = winlng - t_ind;
% nt = N(:,t_ind);
% sum = 0;
% if((t1(t_ind)-T) < 0)
% tminT_ind = 1;
% yk((tminT_ind+diff):winlng,:) = y_zoh(tminT_ind:t_ind,:);
% ny(t_ind,1) = 0;
% elseif((t1(ind2)-T)>=0)
% tminT_ind = t_ind-winlng+1;
% yk = y_zoh(tminT_ind:t_ind,:);
%
%
% y_temp = unique(yk);
% t_dis_temp = t_dis(t_dis > t1(tminT_ind) & t_dis < t1(t_ind));
% limits = t_dis_temp - t1(tminT_ind);
% limits = [limits;T];
%
% tau_1 = 1;
% for i = 1:N_k
% tau_2 = find(abs(tau - limits(i)) < 1e-6);
% sum = sum + y_temp(i)*trapz(tau(tau_1:tau_2),nt(tau_1:tau_2));
% tau_1 = tau_2;
% end
%
% ny(t_ind,1) = sum;
% end
% end
% figure
% plot(t1,ny)
%% calculating and ploting the fault f_1, f_2,...., f_nf
% FHAT is the result of the equation for the coefficients
% vartheta = -<n,y>- <m,Bu>
FHAT = NY + mB;
for k = 1:length(t1)
f_Hat(k,:) = P_f(k)*FHAT(k,:);
end
for i = 1:n_f
subplot(n_f,1,i)
hold on;
plot(t_dis,f(:,i),'-.','LineWidth',3);
plot(t1,f_Hat(:,i),'LineWidth',3);
hold off;
end
\ No newline at end of file
%% Simulation for fault identification of a linear time variant system of the form
% The fault and the disturbance are assumed to be described by a signal
% model of the form
% | x(t) | |A GP_d E_1P_f | | x(t) | | B |
%d/dt | v_d(t) | = |0 S_d 0 | | v_d(t) | + | 0 |u(t)
% | v_f(t) | |0 0 S_f | | v_f(t) | | 0 |
%
% | x(t) |
% y(t) = |C 0 E_2P_f | | v_d(t) |
% | v_f(t) |
%% Define the simulation time t_end, the interval length T and the discretization step dt
T = 1;
dt = 1e-2;
t_end = 3*T;
t =(-T:dt:t_end)';
tau = (0:dt:T);
%% Define System Dynamic
% the system matrices A(t), B(t), C(t), G(t), E_1(t), E_2(t)
% the matrices are defined as function handle matrices
A = @(t) [ -2 , 0 ;
0 , -4];
B = @(t) [1;
0];
C = @(t) [ -2 ,-3 ];
G = @(t) [ 0 ;
1 ];
E_1 = @(t) [ -7 ;
0 ];
E_2 = @(t) [0];
%% Define the fault signal model
% d/dt v_f(t) = S_f(t)v_f(t)
% f(t) = P_f(t)v_f(t)
S_f = @(t) 0;
P_f = @(t) 1;
%% Define the disturbance signal model
% d/dt v_d(t) = S_d(t)v_d(t)
% d(t) = P_d(t)v_d(t)
w0 = 2;
S_d = @(t) [0 , 1 ;
-(w0)^2 , 0];
P_d = @(t) [1 , 0 ];
%% define the extended System
n_x = size(A(1),1 );
n_u = size(B(1),2 );
n_y = size(C(1),1 );
n_d = size(G(1),2 );
n_f = size(E_1(1),2);
n_vd = size(S_d(1),1);
n_vf = size(S_f(1),1);
A_E = @(t) [A(t) , G(t)*P_d(t) , E_1(t)*P_f(t);
zeros(n_vd,n_x) , S_d(t) , zeros(n_vd,n_vf);
zeros(n_vf,n_x) , zeros(n_vf,n_vd), S_f(t)];
B_E = @(t) [B(t); zeros(n_vd,n_u); zeros(n_vf,n_u)];
C_E = @(t) [C(t), zeros(n_y,n_vd), E_2(t)*P_f(t)];
n_E = size(A_E(1),1);
%% Determining the Output
% here enter the intial values of the system
t1 =(0:dt:t_end);
opts = odeset('RelTol',1e-6,'AbsTol',1e-9);
fac = 20;
dt_dis = fac*dt;
t_dis = 0:dt_dis:t_end;
u = @(t) exp(-t).*sin(t);
%u = @(t) 0;
xE = ones(n_E,1);
[t_dis, xE ] = ode45(@(t,xE) A_E(t)*xE + B_E(t)*u(t), t_dis,[zeros(n_x,1);0;10;6], opts);
v_d = xE(:,(n_x+1):(n_vd+n_x)).';
v_f = xE(:,(n_vd+n_x+1):n_E).';
for k = 1:length(t_dis)
d(k,:) = P_d(t_dis(k)) * v_d(:,k);
y(k,:) = C_E(t_dis(k)) * xE(k,:).';
f(k,:) = P_f(t_dis(k)) * v_f(:,k);
end
% figure
% hold on
% plot(t_dis,f,'--');
% plot(t_dis,y,':','LineWidth',3);
% plot(t_dis,d);
% hold off
% t1 = t1.';
%% Discretization of the -A_E.' and C_E matrices for the whole simualtion interval [-T, t_end]
for k = 1:size(t,1)
Aek(k,:,:) = -A_E(t(k)).';
Ce_Glob(:,:,k)= C_E(t(k)).';
end
%% Simulating the Transition matrix for the whole simualtion interval [-T, t_end]
Phi = misc.fundamentalMatrix.odeSolver_par(Aek, t);
% n(tau,t) =
% C_E.'(tau,t)Phi.'(tau,tau_0,t)W^(-1)(tau_0,T,t)Phi(tau_0,T,t)m_E,i1
% m_E = Phi(tau,tau_0,t)int(Phi(tau_0,tau,t)C_E(tau,t)n(tau,t),tau,0,tau)
% with the controlabillity Gramian
% W(tau_0,T,t) =
% int(Phi(tau_0,tau,t)C_E(tau,t)C_E.'(tau,t)Phi.'(tau_0,tau,t),tau,0,T)
winlng = size(tau,2);
I = eye(n_vf);
N = zeros(winlng,size(t1,2),n_y,n_vf);
m_E = zeros(n_E,length(tau),length(t1),n_vf);
% The only a segment of the Phi matrix on the interval [t-T,t] is extracted
% and the calculation for m_E and n is done numerically for the interval [0, t_end]
for indtMinT = 1:size(t1,2)
indt = indtMinT + winlng -1;
Phitau = Phi(indtMinT:indt,indtMinT,:,:);
Ce_loc = Ce_Glob(:,:,indtMinT:indt);
%% Calculating W numerically
for k = 1:length(tau)
Phitt0(:,:,k) = Phitau(k,1,:,:);
Phit0t = inv(Phitt0(:,:,k));
w(k,:,:) = Phit0t*Ce_loc(:,:,k)*(Ce_loc(:,:,k).')*(Phit0t.');
end
tmp = reshape(w, length(tau), n_E^2);
tmp_I = trapz(tau, tmp);
W = reshape(tmp_I, n_E , n_E);
%% Calculating n denoted as N because its a matrix :)
PhiTt0(:,:,1) = Phitau(end,1,:,:);
Phit0T = inv(PhiTt0);
for k = 1:length(tau)
Phitt0(:,:,k) = Phitau(k,1,:,:);
Phit0t = inv(Phitt0(:,:,k));
for ki = 1:n_vf
N(k,indtMinT,:,ki) = (Ce_loc(:,:,k).')*(Phit0t.')*inv(W)*(Phit0T*[zeros(n_E-n_vf,1);I(:,ki)]);
end
% N(tau,t,n_y,r_sum)
end
%% calculating the states m_E
for ki = 1:n_vf
m_E(:,:,indtMinT,ki) = calculateStates(dt,tau,Phitau,Ce_loc,N(:,indtMinT,:,ki));
end
% m_E(nk,tau,t,r_sum)
end
%% calculating < m_E,B_Eu >
u_dis = u(t_dis);
u_zoh = repelem(u_dis,fac);
u_zoh = u_zoh(1:length(t1));
for ki = 1:n_vf
mB(:,ki) = kernTransformTwo(t,t1,tau,m_E(:,:,:,ki),B_E,u_zoh);
end
%% calculating < n,y >
% y is assumed to be zero on the interval from [-T,0] and a segment of y is
% extracted on the interval from [t-T,t] and then multiplied with n for
% every t, the result is stored in NY
y_zoh = repelem(y,fac);
y_zoh = y_zoh(1:length(t1));
for ind2 = 1:length(t1)
Summe = zeros(n_vf,1);
yk = zeros(winlng,n_y);
diff = winlng - ind2;
if((t1(ind2)-T) < 0)
ind1 = 1;
yk((ind1+diff):winlng,:) = y_zoh(ind1:ind2,:);
elseif((t1(ind2)-T)>=0)
ind1 = ind2-winlng+1;
yk = y_zoh(ind1:ind2,:);
end
for j = 1:n_y
for ki = 1:n_vf
Summe(ki,1) = trapz(tau,N(:,ind2,j,ki).*yk(:,j)) + Summe(ki,1);
end
end
NY(ind2,:) = Summe;
end
% NY(t,r_sum)
%% calculating ZOH
% delta_t = dt_dis;
% N_k = find(t_dis == T)-1;
%
%
% for t_ind = 1:length(t1)
% yk = zeros(winlng,n_y);
% diff = winlng - t_ind;
% nt = N(:,t_ind);
% sum = 0;
% if((t1(t_ind)-T) < 0)
% tminT_ind = 1;
% yk((tminT_ind+diff):winlng,:) = y_zoh(tminT_ind:t_ind,:);
% ny(t_ind,1) = 0;
% elseif((t1(ind2)-T)>=0)
% tminT_ind = t_ind-winlng+1;
% yk = y_zoh(tminT_ind:t_ind,:);
%
%
% y_temp = unique(yk);
% t_dis_temp = t_dis(t_dis > t1(tminT_ind) & t_dis < t1(t_ind));
% limits = t_dis_temp - t1(tminT_ind);
% limits = [limits;T];
%
% tau_1 = 1;
% for i = 1:N_k
% tau_2 = find(abs(tau - limits(i)) < 1e-6);
% sum = sum + y_temp(i)*trapz(tau(tau_1:tau_2),nt(tau_1:tau_2));
% tau_1 = tau_2;
% end
%
% ny(t_ind,1) = sum;
% end
% end
% figure
% plot(t1,ny)
%% calculating and ploting the fault f_1, f_2,...., f_nf
% FHAT is the result of the equation for the coefficients
% vartheta = -<n,y>- <m,Bu>
FHAT = NY + mB;
for k = 1:length(t1)
f_Hat(k,:) = P_f(k)*FHAT(k,:);
end
figure
for i = 1:n_f
subplot(n_f,1,i)
hold on;
plot(t_dis,f(:,i),'-.','LineWidth',3);
plot(t1,f_Hat(:,i),'LineWidth',3);
hold off;
end
\ No newline at end of file
%% Simulation for fault identification of a linear time variant system of the form
% The fault and the disturbance are assumed to be described by a signal
% model of the form
% | x(t) | |A GP_d E_1P_f | | x(t) | | B |
%d/dt | v_d(t) | = |0 S_d 0 | | v_d(t) | + | 0 |u(t)
% | v_f(t) | |0 0 S_f | | v_f(t) | | 0 |
%
% | x(t) |
% y(t) = |C 0 E_2P_f | | v_d(t) |
% | v_f(t) |
%% Define the simulation time t_end, the interval length T and the discretization step dt
T = 1;
dt = 1e-2;
t_end = 3*T;
t =(-T:dt:t_end)';
tau = (0:dt:T);
%% Define System Dynamic
% the system matrices A(t), B(t), C(t), G(t), E_1(t), E_2(t)
% the matrices are defined as function handle matrices
A = @(t) [ -2 , 0 ;
0 , -4];
B = @(t) [1;
0];
C = @(t) [ -2 ,-3 ];
G = @(t) [ 0 ;
1 ];
E_1 = @(t) [ -7 ;
0 ];