% Init parameters
clear all;close all;clc

g=9.81; % m/s^2
m=0.486; % kg
l=0.25; % m 
d=3.232e-7; % kg*m^2
k=2.9842e-5; % kg*m
Ix=3.83e-3; % kg*m^2
Iy=3.83e-3; % kg*m^2
Iz=7.66e-3; % kg*m^2

%% Point d'équilibre

xbar = [0;0;-5;zeros(9,1)];
%wbar = ?? % à compléter
%ubar = wbar*ones(4,1);
% vérification du point d'équilibre en simulation

%% Modèle linéarisé

% [At,Bt,Ct,Dt]=linmod('quad_simu_lin_etu',xbar,ubar);
% Ct=Ct([1:3 9],:);
% Dt=zeros(4);
% P=ss(At,Bt,Ct,Dt);

%% Trajectoire autour du point d equilibre

t_traj = [10    25    55    70   100   115   145   160   190]; % (txi txf tyi tyf tzi tzf tpsii tpsif]
v_traj = [0 15 0 10 0 1 0 pi/10]; % (txi txf tyi tyf tzi tzf tpsii tpsif]
coef_traj = zeros(4,6);
coef_traj(1,:)=interp_poly5(v_traj(1),v_traj(2),t_traj(1),t_traj(2));
coef_traj(2,:)=interp_poly5(v_traj(3),v_traj(4),t_traj(3),t_traj(4));
coef_traj(3,:)=interp_poly5(v_traj(5),v_traj(6),t_traj(5),t_traj(6));
coef_traj(4,:)=interp_poly5(v_traj(7),v_traj(8),t_traj(7),t_traj(8));

%% Chargement de la loi de commande

load K.mat

%% Détection et localisation de défauts

% Te=0.01;
% Pe=c2d(P,Te,'zoh');
% [A,B,C,D]=ssdata(Pe);