% Clears all variables from the workspace to ensure a clean environment.
clear all;

%% Cart-pole system parameters
m = 0.1;        % mass of the pendulum bob (kg)
L = 1;          % length of the pendulum rod (m)
g = 9.81;       % acceleration due to gravity (m/s^2)
M = 1;          % mass of the cart (kg)
d1 = 1;         % damping coefficient of the cart
d2 = 0.5;       % damping coefficient of the pendulum

%% Linearization of the system around the theta = 0 (upright position)
q_ = (m+M) * g / (M*L);  % combined gravitational term
% State-space representation of the linearized system:
A = [0, 1, 0, 0; 
     0, -d1, -g*m/M, 0; 
     0, 0, 0, 1; 
     0, d1/L, q_, -d2];
B = [0, 1.0/M, 0, -1/(M*L)].';   % input matrix (transposed)
C_angle = [0, 0, 1, 0];          % output matrix for angle measurement
C_cart = [1, 0, 0, 0];           % output matrix for cart position measurement
D = 0;                           % direct transmission (none in this case)

%% Luenberger observer design
% Desired poles for the observer
p = 2.5 * [-10, -2, -5, -20];    
% Constructing the observer gain matrix using pole placement
C = [C_cart; C_angle];           % Output matrix combining cart and angle
K_luen = place(A.', C', p);      % Observer gain matrix

%% Definition of noise characteristics
r1 = 1e-3;                       % Variance of observation noise for cart position
r2 = 5e-4;                       % Variance of observation noise for pendulum angle

q1 = 5e-4;                       % Variance of process noise for cart velocity
q2 = 1e-3;                       % Variance of process noise for pendulum angular velocity

% System initial conditions
x0 = [0, 0, 0.5, 0.1];           % Initial state [cart position, cart velocity, pole angle, pole angular velocity]

% Initial estimated process noise covariance
P_k_init = .01;

%% Optimal control design (Linear Quadratic Regulator - LQR)
% Continuous time LQR design
R_ctrl = 10;                     % Weight on the control effort
Q_ctrl = diag([10, 5, 20, 10]);  % State error weighting

% Solve the continuous-time algebraic Riccati equation (CARE)
[X, K_lqr, L] = icare(A, B, Q_ctrl, R_ctrl);
% there exists a more recent iCARE function but be very careful in change
% the code since the order of the outputs is different
Tsim = 1e-3;                     % Simulation time step

% Discrete time LQR design
Ts = 0.01;                       % Sampling time
% Discretize the system using zero-order hold (ZOH) method
Ad = expm(A*Ts);                 % Discrete-time A matrix
Bd = pinv(A)*(Ad-eye(4))*B;      % Discrete-time B matrix

% Solve the discrete-time algebraic Riccati equation (DARE)
[X, K_lqr_d, L] = dare(Ad, Bd, Q_ctrl, R_ctrl);

%% Frequency analysis (optional, not used in control but for analysis)
% Transfer functions derived from state-space models for frequency response analysis
[b_angle, a_angle] = ss2tf(A, B, C_angle, D);
[b_cart, a_cart] = ss2tf(A, B, C_cart, D);

% Create transfer function models for angle and cart position
sys_angle = tf(b_angle, a_angle);
sys_cart = tf(b_cart, a_cart);