% ME 2046 MIDTERM
% DANE SABO
% MARCH 17TH, 2025
% PROBLEM 5
close all
clear all

A = [1 0.09; 0 0.9];
B = [0.005; 0.1];
C = [1 0];
D = 0;
T = 0.1; %s

fprintf("\n======= Part 1: =========\n")
p = [1 -1.776 0.819];
fprintf("Closed loop poles: \n")
lambdas = roots(p)

fprintf("Natural Frequency: \n")
omega_n = sqrt(p(3))

fprintf("Damping Ratio: \n")
zeta = abs(p(2)/2/omega_n)

fprintf("\n======= Part 2: =========\n")
lambdas(:)
K = place(A, B, lambdas(:))

fprintf("Check eigenvalues...\n")
eig(A - B*K)
fprintf("Great! \nNow finding N...")
N = 1/(C*inv(eye(2) - A + B*K)*B)

fprintf("\n======= Part 3: =========\n")
sys = ss(A-B*K, B*N, C, D, T)
G_CL = tf(sys)

% Define simulation parameters
n = 50;                     % number of steps
t = 0:T:(n-1)*T;            % time vector for discrete time instants
u = ones(n,1);              % step input (magnitude 1 at each step)
x_0 = [-0.5; 0];            % initial condition

% Simulate the discrete system response using dlsim
[y, x] = dlsim(A-B*K, B*N, C, D, u, x_0);

% Plot the discrete step response using stairs for clarity
figure;
stairs(t, y, 'LineWidth', 1.5)
xlabel('Time')
ylabel('Output')
title('Discrete Step Response with Initial Conditions')
grid on

fprintf("\n======= Part 4: =========\n")
figure;
bode(sys)

fprintf("\n======= Part 5: =========\n")
witness_me = tf(1, [1, -1.638, 0.617], T);
%WITNESS ME BLOOD BAG! WITNESSSS!!!! *spray paints mouth*
observer = ss(witness_me)

fprintf("\n======= Part 6: =========\n")
fprintf("Observer poles: \n")
eig(observer.A)

fprintf("Closed-Loop poles: \n")
eig(A - B*K)
mag = sqrt(0.888^2 +0.1745^2)

fprintf("The observer has an unstable pole while the closed loop system has stable\n")
fprintf("poles within the unit circle. This observer will not work, because \n")
fprintf("the error will not converge to the true states.\n")

fprintf("\n======== Part 7: =========\n")
fprintf("Let's make our closed loop observer poles 10 times faster than \n")
fprintf("the original system.\n")
fprintf("Going to the w-plane... \n")
sys_w = d2c(sys, 'tustin');
cl_poles_w = eig(sys_w)
observer_poles_w = 10 * cl_poles_w
fprintf("Now going back to the z-plane...")
observer_poles_z = exp(observer_poles_w*T)

fprintf("and finally using place to get L...")
L = transpose(place(A', C', observer_poles_z))