actuator.m

% --------System Matrices---------%
A1=[0 1 0 0 ; 0 -16 -4.53 0 ; 0 0 0 1; 0 52.46 47.06 0];
B1=[0;3.78;0;-12.39];
C1=[1 0 0 0  ;0 0 1 0 ];
D1=[0;0];
d=[0 ; 0];
E=[1;1;1;1];
H=[0; 1;0; -1];
%-----Conversion to Discrete time model----%
[A B C D]=c2dM(A1,B1,C1,D1,.01);
%-----feed back controller----%
L=[-23.2787  -27.4554 -114.8834  -11.4960];
%----History of states, inputs and outputs----%
s=4;
t=1000;
x=zeros(4,1001);
y=zeros(2,1001);
u=zeros(1,1001);
x(:,1)=[1;1;1;1];
u(:,1)=-L*x(:,1);
y(:,1)=C*x(:,1)+D*u(:,1);
for i=1:t;
    x(:,i+1)=A*x(:,i)+B*u(:,i);
    u(:,i+1)=-L*x(:,i+1);
    y(:,i+1)=C*x(:,i+1)+D*u(:,i+1);
end
Y=zeros(10,501);
for i=1:501
    Y(:,i)=[y(:,i);y(:,i+1);y(:,i+2);y(:,i+3);y(:,i+4)];
end
U=zeros(5,501);
for i=1:501
    U(:,i)=[u(:,i);u(:,i+1);u(:,i+2);u(:,i+3);u(:,i+4)];
end
%----Calculation of different Fault distribution matices----%
Q1=[D;C*B;C*A*B;C*A^2*B;C*A^3*B];
Q2=[zeros(size(D));D;C*B;C*A*B;C*A^2*B];
Q3=[zeros(size(D));zeros(size(D));D;C*B;C*A*B];
Q4=[zeros(size(D));zeros(size(D));zeros(size(D));D;C*B];
Q5=[zeros(size(D));zeros(size(D));zeros(size(D));zeros(size(D));D];
Q=[Q1 Q2 Q3 Q4 Q5];
%-----Actuator Fault Distribution matrix---%
Qa1=[D;C*B;C*A*B;C*A^2*B;C*A^3*B];
Qa2=[zeros(size(D));D;C*B;C*A*B;C*A^2*B];
Qa3=[zeros(size(D));zeros(size(D));D;C*B;C*A*B];
Qa4=[zeros(size(D));zeros(size(D));zeros(size(D));D;C*B];
Qa5=[zeros(size(D));zeros(size(D));zeros(size(D));zeros(size(D));D];
Qa=[Qa1 Qa2 Qa3 Qa4 Qa5];
%-----Componenet Fault Distribution matrix---%
Tc1=[d;C*H;C*A*H;C*A^2*H;C*A^3*H];
Tc2=[zeros(size(d));d;C*H;C*A*H;C*A^2*H];
Tc3=[zeros(size(d));zeros(size(d));d;C*H;C*A*H];
Tc4=[zeros(size(d));zeros(size(d));zeros(size(d));d;C*H];
Tc5=[zeros(size(d));zeros(size(d));zeros(size(d));zeros(size(d));d];
Tc=[Tc1 Tc2 Tc3 Tc4 Tc5];
%-----Diturbance Distribution matrix---%
Td1=[d;C*E;C*A*E;C*A^2*E;C*A^3*E];
Td2=[zeros(size(d));d;C*E;C*A*E;C*A^2*E];
Td3=[zeros(size(d));zeros(size(d));d;C*E;C*A*E];
Td4=[zeros(size(d));zeros(size(d));zeros(size(d));d;C*E];
Td5=[zeros(size(d));zeros(size(d));zeros(size(d));zeros(size(d));d];
Td=[Td1 Td2 Td3 Td4 Td5];
R=[C;C*A;C*A^2;C*A^3;C*A^4];
%-----Calculation of weight matrix 'w'---%
w=null(R');
w_x=[0;-3.2865;0;9.5935;0;-8.3093;0;1;0;1];
w_phi=[.8521;0;-3.5605;0;5.5651;0;-3.8567;0;1;0];
w_a=[-.0549;-.0168;1;.3050;-.8352;-.2555;-1.1099;-.3394;1;.3049];
w_mp=[15.7830;.0006;-10.0001;.0073;-25.7829;0.0110;10.;.0046;10;.0004];
w_Mc=[4.8008;1.4639;-3.3294;-1.0199;-6.4689;-1.9786;3.7230;1.1340;1.2746;.3886];

%------Fault Matrices----%fault=zeros(1,501);
fault=zeros(1,501);
Fa=zeros(5,501);
Fc=zeros(5,501);
Dt=zeros(5,501);
S=zeros(10,501);
%------Fault Creation in position sensor----%
fault(:,350:501)=1;
Fa(1,:)=fault;
%-----Residual Calculation---%
r_x=w_x'*((Y-Q*U)-Qa*Fa-Tc*Fc-Td*Dt-S);
r_phi=w_phi'*((Y-Q*U)-Qa*Fa-Tc*Fc-Td*Dt-S);
r_a=w_a'*((Y-Q*U)-Tc*Fc-Qa*Fa-Td*Dt-S);
r_mp=w_mp'*((Y-Q*U)-Tc*Fc-Qa*Fa-Td*Dt-S);
r_Mc=w_Mc'*((Y-Q*U)-Qa*Fa-Tc*Fc-Td*Dt-S);
%-----Plot of Residuals---%
figure;
rest=[0:.01:5];
l=ones(1,501);
subplot(3,2,1);plot(rest,r_x,rest,1e-3*l,'r--',rest,-1e-3*l,'r--'),grid, ylabel('res_x'),title('Residual for Position Sensor') ;
subplot(3,2,2);plot(rest,r_phi,rest,.5e-4*l,'r--',rest,-.5e-4*l,'r--'),grid, ylabel('res_phi'),title('Residual for Angle Sensor') ;
subplot(3,2,3);plot(rest,r_a,rest,2.5e-4*l,'r--',rest,-2.5e-4*l,'r--'),grid, ylabel('res_a'),title('Residual for Actuator') ;
subplot(3,2,4);plot(rest,r_mp,rest,2.5e-3*l,'r--',rest,-2.5e-3*l,'r--'),grid, ylabel('res_mp'),title('Residual for mp') ;
subplot(3,2,5);plot(rest,r_Mc,rest,2.5e-4*l,'r--',rest,-2.5e-4*l,'r--'),grid, ylabel('res_Mc'),title('Residual for Mc') ;
subplot(3,2,6);plot(rest,fault),grid, ylabel('fault'),xlabel('time'),title('Fault in Actuator') ;