%% Lab skeleton for lab 1 in TSFS06: Linear residual generation

%% short description of the simulation model
% Variables saved in the workspace after simulation:
% t   time vector
% ref reference signal to the controller (not used here)
% y   measurement signals
% f   fault signals
% res residuals
% T   Thresholded residuals (with threshold 1)
%
% Variables from the workspace that is used in the simulation model:
% Fc     State-space model of the controller, computed in controllerdesign.m 
% Model parameters J1, J2, k, alpha1, alpha2, alpha3, alpha4
% Tfault Time of failure injection. Default: 25
% NP     Defines noise power. Default value: NP=0.00005
% R      State-space model for the residual generator.
%        Code for this is done by student in this file.
% Rseed  Random seed for noise generation

clear
addpath /courses/TSFS06/polynomial/ % Linux

pinit

% Set the random seed
Rseed = floor(sum(100*clock));

% Define model constants. Make sure these are not overwritten in the code
% below. These values are used in the simulation model.
J1     = 1;
J2     = 0.5;
k      = 99/90;
alpha1 = 1;
alpha3 = 0.1;
alpha2 = 0.05;
alpha4 = 0.1;

% Define model matrices H, L, and F.
H = [];
L = [];
F = [];

%% Verify that the model is correctly formulated
% For this to work, make sure that the order of the signals in the z and f 
% vectors is correct.
% z = (y1, y2, y3, u ) where y1, y2, and y3 are the sensors measuring
% theta1, theta1', and theta2
% f = (f1,f2,f3,f4)

lab1_modelcheck(H,L) % Verify that the nominal model is correct
lab1_modelcheck(H,L,F) % Verify that the model with faults is correct

%% Controller design
controllerdesign;

%% Noise power for simulating measurement noise
NP = 0.00005; % Set to 0 to simulate without noise
Tfault = 25;

%% Residual generator design
% Put your code here, name the residual generators R1, R2, etc.
%
% R = ss(Ra,Rb,Rc,Rd);
R1 = ss([0 0 0 0]); % Dummy residual generator
R2 = ss([0 0 0 0]); % Dummy residual generator

% Collect all residual generators into one single state-space object.
% The object must be called R for the simulation model to work.

R = [R1;R2];

%% Simulate
% Set fi=0 i=1,..,4 for the fault free case. Set auitable values for
% when faults are simulated.
f1=0;
f2=0;
f3=0;
f4=0;

% Simulation is started either by choosing the menu
% Simulation->Start in the Simulink window of by executing the code below
sim('TSFS06lab1');

%% Calculate the single fault diagnoses
% Define decision matrices for when residuals are over and under the
% thresholds respectively.
s0 = ones(2,5); % dummy exempel
s1 = [0 0 1 1 1;0 1 0 1 1]; % dummy exempel

% Calculate the diagoses
[S,alarm] = decisioncalc(T,s0,s1);

%% Plota and evaluate
figure(100)
plot( t, y ) % Plot
ylabel('utsignaler')
xlabel('t')
legend('y_1','y_2','y_3','Location','SE')

figure(101)
subplot(211)
plot( t, res(:,1))
ylabel('residual 1')
xlabel('t')

subplot(212)
plot( t, res(:,2))
ylabel('residual 2')
xlabel('t')

figure(102)
plot( t, alarm)
title('alarm')
xlabel('t')

figure(103)
subplot(231)
plot(t,S(:,1));
ylabel('NF indicator');
xlabel('t')
axis([min(t) max(t) -0.3 1.3]);

subplot(232)
plot(t,S(:,2));
ylabel('F_1 indicator');
xlabel('t')
axis([min(t) max(t) -0.3 1.3]);

subplot(233)
plot(t,S(:,3));
ylabel('F_2 indicator');
xlabel('t')
axis([min(t) max(t) -0.3 1.3]);

subplot(234)
plot(t,S(:,4));
ylabel('F_3 indicator');
xlabel('t')
axis([min(t) max(t) -0.3 1.3]);

subplot(235)
plot(t,S(:,5));
ylabel('F_4 indicator');
xlabel('t')
axis([min(t) max(t) -0.3 1.3]);
