% Author: Tomas Trafina, Bc., system control/robotics field
% Czech Technical University in Prague, Dept. of Control Engineering
% email address: ttrafina@gmail.com
% May 2018; Last revision: 17-May-2018

%------------- BEGIN CODE --------------

%% physical constants
env.g=9.81;                     % gravitational acceleration
env.h=150;                      % approximation of operational height above sea level
env.rho0=1.2256;                % air density at sea level, temperature od 20 °C
uav.m=7;                        % take off weight of the UAV
uav.R=0.6;                      % distance between geometrical center of the UAV and a motor's center
uav.S=[0.15 0.15 0.21];         % UAV's air resistant area for corresponding axes
uav.I=[0.65 0.65 1.33];         % UAV's moments of inertia for corresponding axes
uav.k=0.0997;                   % ratio coefficient between motors thrust and torque
uav.alpha=pi/3;                 % angle between the arms
uav.Rx=uav.R*sin(uav.alpha);    % distance of front motors from axis Y
uav.Ry=uav.R*cos(uav.alpha);    % distance of front motors from axis X
clear uav.alpha

%% linear continuous-time model
A1=[0 -env.g 0; env.g 0 0;  0 0 0];     % part of matrix A
A=[zeros(3,12);     % transition matrix
    zeros(3,6) A1 zeros(3);  
    eye(3) zeros(3,9);
    zeros(3) eye(3) zeros(3,6)];
B=[uav.Ry*uav.I(1) uav.Ry*uav.I(1) -uav.Ry*uav.I(1) -uav.Ry*uav.I(1) 0 0;   % input matrix
    uav.Rx*uav.I(2) uav.Rx*uav.I(2) uav.Rx*uav.I(2) uav.Rx*uav.I(2) -uav.R*uav.I(2) -uav.R*uav.I(2);
    -uav.k*uav.I(3) uav.k*uav.I(3) -uav.k*uav.I(3) uav.k*uav.I(3) -uav.k*uav.I(3) uav.k*uav.I(3);
    zeros(2,6);
    -1/uav.m -1/uav.m -1/uav.m -1/uav.m -1/uav.m -1/uav.m;
    zeros(6,6)];
C=[zeros(3,9) eye(3); zeros(1,8) 1 zeros(1,3)];     % output matrix
D=zeros(4,6);   % feedthrough
sysc=ss(A,B,C,D);
clear A1 A B C D

%% state feedback
% extended system with new states for regulation
Af=[sysc.A zeros(12,4); -sysc.C zeros(4)];
Bf=[sysc.B; zeros(4,6)];
% desired positions of system's poles
pf=[-1 -1 -1 -2 -2 -2 -1.5 -1.5 -1.5 -2.5 -2.5 -2.5 -3 -3 -3 -3];

Kf=place(Af,Bf,pf);     % state feedback matrix
reg.Ks=Kf(1:6,1:12);
reg.Ki=Kf(1:6,13:16);
clear Af Bf pf Kf

%% sampling
sensor.TSimu=0.004;     % sampling time of IMU
sensor.TSrtk=0.05;      % sampling time of RTK GNSS sensor

sensor.covImu=[1.355 1.291 1.257 0.0127 0.0124 0.0124 0.0011 0.0012 0.9615]*10^(-4);    % variances of IMU measurements
sensor.covRtk=[0.1314;0.2922;0.7292]*10^(-4);   % variances of RTK GNSS measurements
sensor.Rimu=diag(sensor.covImu);    % variance matrix of IMU
sensor.Rrtk=diag(sensor.covRtk);    % variance matrix of RTK GNSS sensor