├── Ball_Beam_System_LQR_Design ├── Beam_Ball_LQR.m └── Project_Beam_Ball_System_LQR_Design.m ├── Ball_Beam_System_Regulator_Design ├── Beam_Ball_Regul_Proj.m └── Project_Beam_Ball_System_Regulator_Design.m ├── Ball_Beam_System_Regulator__Phi_Full_Observer_Design ├── Beam_Ball_Phi_Full_Obser_Regual_Proj.m ├── Beam_Ball_Regul_FO_Proj.m └── Project_Beam_Ball_System_Regulator_Phi_Full_Observer_Design.m ├── Ball_Beam_System_Regulator__x_Full_Observer_Design ├── Beam_Ball_Regul_FO_Proj.m ├── Beam_Ball_x_Full_Obser_Regual_Proj.m └── Project_Beam_Ball_System_Regulator_x_Full_Observer_Desig.m ├── Ball_Beam_System_Servo_Adding_Integrator ├── Beam_Ball_Servo_Add_Int_Proj.m └── Project_Beam_Ball_System_Servo_Add_Int_Design.m ├── Ball_Beam_System_Servo_Adding_Integrator_Full_Order_Observer ├── Beam_Ball_Full_Obser_Servo_Add_Int_Proj.m ├── Beam_Ball_Servo_Add_Int_FO_Obs_Proj.m └── Project_Beam_Ball_System_Servo_Adding_Integrator_FO_Obs.m ├── Ball_Beam_System_Servo_Adding_Integrator_Minimum_Order_Observer ├── Beam_Ball_Min_Obser_Servo_Add_Int_Proj.m ├── Beam_Ball_Servo_Add_Int_MO_Obs_Proj.m └── Project_Beam_Ball_System_Servo_Adding_Integrator_MO_Obs.m ├── Ball_Beam_System_Servo_Feed_Forward ├── Beam_Ball_Servo_FF_Proj.m └── Project_Beam_Ball_System_Servo_Feed_Forward.m ├── Ball_Beam_System_Servo_Feed_Forward_Full_Order_Observer ├── Beam_Ball_Full_Obser_Servo_FF_Proj.m ├── Beam_Ball_Servo_FF_FO_Obs_Proj.m └── Project_Beam_Ball_System_Servo_FF_FO_Obs.m ├── Ball_Beam_System_Servo_Feed_Forward_Minimum_Order_Observer ├── Beam_Ball_Min_Obser_Servo_FF_Proj.m ├── Beam_Ball_Servo_FF_MO_Obs_Proj.m └── Project_Beam_Ball_System_Servo_FF_MO_Obs.m ├── CLOS_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration ├── Initial_Data.m ├── Plots_Problme_2_Part_C.m └── Problem_2_Part_C.slx ├── Continous_Time_Inverted_Double_Pendulum_System_Regulator_Design ├── MIMO_DOuble_Pendulum_Regul_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Regulator_Design.m ├── Continous_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design ├── MIMO_Double_Pendulum_Obser_Regul_Proj.m ├── MIMO_Double_Pendulum_Regul_Obs_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Regulator_Observer_Design.m ├── Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator ├── MIMO_Double_Pendulum_Servo_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Servo_Design.m ├── Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer ├── MIMO_Double_Pendulum_Obser_Servo_Proj.m ├── MIMO_Double_Pendulum_Servo_Obs_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Servo_Observer_Design.m ├── Discerete_Time_Inverted_Double_Pendulum_System_Regulator_Design ├── MIMO_DOuble_Pendulum_Regul_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Regulator_Design.m ├── Discerete_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design ├── MIMO_Double_Pendulum_Regul_Obs_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Regulator_Observer_Design.m ├── Discerete_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator ├── MIMO_Double_Pendulum_Servo_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Servo_Design.m ├── Discerete_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer ├── MIMO_Double_Pendulum_Servo_Proj.m └── MIMO_Inverted_Double_Pendulum_System_Servo_Observer_Design.m ├── Discerete_Time_Inverted_Pendulum_System_Regulator_Design ├── Pendulum_Regul_Proj.m └── Project_Inverted_Pendulum_System_Regulator_Design.m ├── Discerete_Time_Inverted_Pendulum_System_Regulator_Observer_Design ├── Pendulum_Regul_Obs_Proj.m └── Project_Inverted_Pendulum_System_Regulator_Observer_Design.m ├── Discerete_Time_Inverted_Pendulum_System_Servo_Adding_Integrator ├── Pendulum_Servo_Add_Int_Proj.m └── Project_Inverted_Pendulum_System_Servo_Adding_Integrator.m ├── Discerete_Time_Inverted_Pendulum_System_Servo_Adding_Integrator_Observer ├── Pendulum_Servo_Add_Int_Obs_Proj.m └── Project_Inverted_Pendulum_System_Servo_Adding_Integrator_Obs.m ├── Effect_of_Data_Transmition_Rate_on_Persuit_Guidance ├── Initial_Data.m ├── Problem_1_Part_G.slx ├── Problem_1_Part_G.slxc └── delT_Variable_Part_G.m ├── Effect_of_Guidance_Time_Constant_on_Persuit_Guidance ├── Initial_Data.m ├── Problem_1_Part_C.slx ├── Problem_1_Part_C.slxc └── T_Variable_Part_C.m ├── Effect_of_Look_Angle_on_Persuit_Guidance ├── Initial_Data.m ├── Problem_1_Part_F.slx ├── Problem_1_Part_F.slxc └── emax_Variable_Part_F.m ├── Effect_of_Saturated_Acceleration_on_Persuit_Guidance ├── Initial_Data.m ├── Problem_1_Part_D.slx ├── Problem_1_Part_D.slxc └── alim_Variable_Part_D.m ├── Effect_of_Seeker_Range_on_Persuit_Guidance ├── Initial_Data.m ├── Problem_1_Part_E.slx ├── Problem_1_Part_E.slxc └── Rmin_Variable_Part_E.m ├── Inverted_Pendulum_System_LQR_Design ├── Pendulum_LQR.m └── Project_Inverted_Pendulum_System_LQR_Design.m ├── Inverted_Pendulum_System_Regulator_Design ├── Pendulum_Regul_Proj.m └── Project_Inverted_Pendulum_System_Regulator_Design.m ├── Inverted_Pendulum_System_Regulator_Full_Observer_Design ├── Pendulum_Full_Obser_Regual_Proj.m ├── Pendulum_Regul_FO_Obs_Proj.m └── Project_Inverted_Pendulum_System_Regulator_Full_Observer_Design.m ├── Inverted_Pendulum_System_Servo_Adding_Integrator ├── Pendulum_Servo_Add_Int_Proj.m └── Project_Inverted_Pendulum_System_Servo_Adding_Integrator.m ├── Inverted_Pendulum_System_Servo_Adding_Integrator_Full_Order_Observer ├── Pendulum_Full_Obser_Servo_Add_Int_Proj.m ├── Pendulum_Servo_Add_Int_FO_Obs_Proj.m └── Project_Inverted_Pendulum_System_Servo_Adding_Integrator_FO_Obs.m ├── Inverted_Pendulum_System_Servo_Adding_Integrator_Minimum_Order_Observer ├── Pendulum_Min_Obser_Servo_Add_Int_Proj.m ├── Pendulum_Servo_Add_Int_MO_Obs_Proj.m └── Project_Inverted_Pendulum_System_Servo_Adding_Integrator_MO_Obs.m ├── Inverted_Pendulum_System_Servo_Feed_Forward ├── Pendulum_Servo_FF_Proj.m └── Project_Inverted_Pendulum_System_Servo_Feed_Forward.m ├── Inverted_Pendulum_System_Servo_Feed_Forward_Full_Order_Observer ├── Pendulum_Full_Obser_Servo_FF_Proj.m ├── Pendulum_Servo_FF_FO_Obs_Proj.m └── Project_Inverted_Pendulum_System_Servo_FF_FO_Obs.m ├── Inverted_Pendulum_System_Servo_Feed_Forward_Minimum_Order_Observer ├── Pendulum_Min_Obser_Servo_FF_Proj.m ├── Pendulum_Servo_FF_MO_Obs_Proj.m └── Project_Inverted_Pendulum_System_Servo_FF_MO_Obs.m ├── LOS_Guidamce_Noneideal_Missile_Dynamics_Lead_Lag_Compensator ├── Initial_Data.m ├── Plots_Problme_1_Part_E.m └── Problem_1_Part_E.slx ├── Lambert_Ballistic_Guidance ├── Initial_Data.m ├── Plots_Problme_1_Part_B.m └── Problem_1_Part_B.slx ├── Lambert_Ballistic_Guidance_Monte_Carlo_Simulation ├── Initial_Data.m ├── Problem_1_Part_C3.slx └── Velocity_Acceleration_Measurement_Uncertinty.m ├── Lambert_Ballistic_Guidance_Secant_Method ├── Initial_Data.m ├── Plots_Problme_1_Part_A.m ├── Problem_1_Part_A.slx └── Secant_Method.m ├── Lead_Angle_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration ├── Initial_Data.m ├── Plots_Problme_3.m └── Problem_3.slx ├── Persuit_Guidance_Noneideal_Missile_Dynamics ├── Initial_Data.m ├── Plots_Problme_1_Part_A.m └── Problem_1_Part_A.slx ├── README.md ├── Sliding_Mode_Control_for_a_Quadotor ├── Initial_Data.m ├── Nonelinear_Control_Project.slx └── Plots.m ├── UAV_Target_Following_and_State_Flow_Implementation ├── Initial_Data.m ├── Plots_Problme_1_Part_A.m └── Problem_1_Part_A.slx ├── UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_HIL ├── Board_Case_2.slx ├── Initial_Data_Case_2.m └── Project_HIL_Case_2.slx ├── UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_SIL ├── Initial_Data_Case_2.m ├── Plots_Case_2.m └── Project_Simulation_Case_2.slx ├── UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_HIL ├── Board_Case_3.slx ├── Initial_Data_Case_3.m └── Project_HIL_Case_3.slx ├── UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_SIL ├── Initial_Data_Case_3.m ├── Plots_Case_3.m └── Project_Simulation_Case_3.slx ├── UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_HIL ├── Board_Case_1.slx ├── Initial_Data_Case_1.m └── Project_HIL_Case_1.slx ├── UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_SIL ├── Initial_Data_Case_1.m ├── Plots_Case_1.m └── Project_Simulation_Case_1.slx └── control project ├── Problem1_2_3.m └── problem1_2_3.slx /Ball_Beam_System_LQR_Design/Beam_Ball_LQR.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_LQR(t,X,u) 2 | 3 | global m g A B J_Beam 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/((1.4)*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | 8 | DX=A*X+B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_LQR_Design/Project_Beam_Ball_System_LQR_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Designing_LQR_Controller_For_Inverted_Pendulum_System_B10_21 2 | clc 3 | clear 4 | global m g J_Beam 5 | 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | 15 | Q = 5*eye(4); 16 | R = 5; 17 | 18 | %% Designing Controller Gain, LQR Method 19 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 20 | r_M = rank(M); 21 | if r_M == min(size(M)) 22 | fprintf('The system is controllable and the rank of M is\n') 23 | disp(r_M) 24 | 25 | [K_reg,P,E]=lqr(A_Linear,B_Linear,Q,R) 26 | 27 | fprintf('The Controller Gain "K" is\n') 28 | disp(K_reg) 29 | else 30 | disp('The System is Unctrollable') 31 | end 32 | 33 | %% Simulation 34 | T = 200; 35 | dt = 0.01; 36 | X0 = [0.2;0;0.174533;0]; 37 | t = 0; 38 | X(:,1) = X0; 39 | Time(1) = t; 40 | k = 1; 41 | while t < T 42 | Xj = X(:,k); 43 | u = -K_reg*Xj; 44 | D1 = Beam_Ball_LQR(t,Xj,u); 45 | D2 = Beam_Ball_LQR(t+dt/2,Xj+D1*dt/2,u); 46 | D3 = Beam_Ball_LQR(t+dt/2,Xj+D2*dt/2,u); 47 | D4 = Beam_Ball_LQR(t+dt,Xj+D3*dt,u); 48 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 49 | X(:,k+1) = Xj; 50 | Time(k+1) = t + dt; 51 | k = k + 1; 52 | t = t + dt; 53 | end 54 | 55 | %% Plots 56 | figure; 57 | subplot(4,1,1);plot(Time,X(1,:)); 58 | title('LQR Controller Design Initial Conddition "0.2,0,10,0" and R equals to "5"') 59 | xlabel('time(s)') 60 | ylabel('x(m)') 61 | 62 | subplot(4,1,2);plot(Time,X(2,:)); 63 | xlabel('time(s)') 64 | ylabel('xdot(m/s)') 65 | 66 | subplot(4,1,3);plot(Time,X(3,:)); 67 | xlabel('time(s)') 68 | ylabel('phi(rad)') 69 | 70 | subplot(4,1,4);plot(Time,X(4,:)); 71 | xlabel('time(s)') 72 | ylabel('phidot(rad/s)') -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator_Design/Beam_Ball_Regul_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Regul_Proj(t,X,u) 2 | 3 | global m g A B J_Beam 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/((1.4)*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator_Design/Project_Beam_Ball_System_Regulator_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Ball_Beam_System_Regulator_Design 2 | clc 3 | clear 4 | 5 | global m g J_Beam 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | C_Yphi = [0 0 1 0]; 15 | 16 | %% Designing Controller Gain, Ackerman Method 17 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 18 | r_M = rank(M); 19 | if r_M == min(size(M)) 20 | fprintf('The system is controllable and the rank of M is\n') 21 | disp(r_M) 22 | 23 | mu_d = [-3 -4 -2+1i -2-1i]; %% Desired Eigenvalues 24 | K_Reg = acker(A_Linear,B_Linear,mu_d); 25 | 26 | fprintf('The Controller Gain "K" is\n') 27 | disp(K_Reg) 28 | else 29 | disp('The System is Unctrollable') 30 | end 31 | 32 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 33 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 34 | r_N_T_Yx = rank(N_T_Yx); 35 | if r_N_T_Yx == min(size(N_T_Yx)) 36 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 37 | disp(r_N_T_Yx) 38 | 39 | muo_d = [-5 -5 -5 -5]; 40 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 41 | Ko_Yx = Ko_T_Yx'; 42 | 43 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 44 | disp(Ko_Yx) 45 | else 46 | disp('The system is unobservable when it observes (x)') 47 | end 48 | 49 | %% Designing Observer Gain When System Observes X3=theta, Ackerman Method 50 | N_T_Yphi = [C_Yphi' A_Linear'*C_Yphi' (A_Linear')^2*C_Yphi' (A_Linear')^3*C_Yphi']; %% N tranpose matrix when system observs X3=theta 51 | r_N_T_Yphi = rank(N_T_Yphi); 52 | if r_N_T_Yphi == min(size(N_T_Yphi)) 53 | fprintf('When system observes (phi), it is Observable and the rank of N_Transpose is\n') 54 | disp(r_N_T_Yphi) 55 | 56 | muo_d = [-5 -5 -5 -5]; 57 | Ko_T_Yphi = acker(A_Linear',C_Yphi',muo_d); 58 | Ko_Yphi = Ko_T_Yphi'; 59 | 60 | fprintf('The Observer Gain "Ko" When System Observes (phi) is\n') 61 | disp(Ko_Yphi) 62 | else 63 | disp('The system is unobservable when it observes (phi)') 64 | end 65 | 66 | %% Simulation 67 | T = 5; 68 | dt = 0.01; 69 | X0 = [0.1;0;0.0872665;0]; 70 | t = 0; 71 | X(:,1) = X0; 72 | Time(1) = t; 73 | k = 1; 74 | while t < T 75 | Xj = X(:,k); 76 | u = -K_Reg*Xj; 77 | D1 = Beam_Ball_Regul_Proj(t,Xj,u); 78 | D2 = Beam_Ball_Regul_Proj(t+dt/2,Xj+D1*dt/2,u); 79 | D3 = Beam_Ball_Regul_Proj(t+dt/2,Xj+D2*dt/2,u); 80 | D4 = Beam_Ball_Regul_Proj(t+dt,Xj+D3*dt,u); 81 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 82 | X(:,k+1) = Xj; 83 | Time(k+1) = t + dt; 84 | k = k+1; 85 | t = t + dt; 86 | end 87 | 88 | %% Plots 89 | figure; 90 | subplot(4,1,1);plot(Time,X(1,:)); 91 | title('Regulator Controller Design Initial Conddition "0.1,0,5,0"') 92 | xlabel('time(s)') 93 | ylabel('x(m)') 94 | 95 | subplot(4,1,2);plot(Time,X(2,:)); 96 | xlabel('time(s)') 97 | ylabel('xdot(m/s)') 98 | 99 | subplot(4,1,3);plot(Time,X(3,:)); 100 | xlabel('time(s)') 101 | ylabel('phi(rad)') 102 | 103 | subplot(4,1,4);plot(Time,X(4,:)); 104 | xlabel('time(s)') 105 | ylabel('phidot(rad/s)') -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator__Phi_Full_Observer_Design/Beam_Ball_Phi_Full_Obser_Regual_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Beam_Ball_Phi_Full_Obser_Regual_Proj(t,Xh,u,y) 2 | global A B C_Yphi Ko_Yphi 3 | 4 | yh = C_Yphi*Xh; 5 | DXh = A*Xh + B*u + Ko_Yphi*(y-yh); -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator__Phi_Full_Observer_Design/Beam_Ball_Regul_FO_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Regul_FO_Proj(t,X,u) 2 | 3 | global A B m g J_Beam 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator__Phi_Full_Observer_Design/Project_Beam_Ball_System_Regulator_Phi_Full_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% %% Project#1_Advanced_Control_Beam_Ball_System_Regulator_Phi_Full_Observer_Design 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yphi Ko_Yphi 6 | %% Designing Controller Gain, Ackerman Method 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yphi = [0 0 1 0]; 14 | 15 | 16 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 17 | r_M = rank(M); 18 | if r_M == min(size(M)) 19 | fprintf('The system is controllable and the rank of M is\n') 20 | disp(r_M) 21 | 22 | mu_d = [-3 -4 -2+1i -2-1i]; %% Desired Eigenvalues 23 | K_Reg = acker(A_Linear,B_Linear,mu_d); 24 | 25 | fprintf('The Controller Gain "K" is\n') 26 | disp(K_Reg) 27 | else 28 | disp('The System is Unctrollable') 29 | end 30 | 31 | %% Designing Observer Gain When System Observes X3=phi, Ackerman Method 32 | N_T_Yphi = [C_Yphi' A_Linear'*C_Yphi' (A_Linear')^2*C_Yphi' (A_Linear')^3*C_Yphi']; %% N tranpose matrix when system observs X3=theta 33 | r_N_T_Yphi = rank(N_T_Yphi); 34 | if r_N_T_Yphi == min(size(N_T_Yphi)) 35 | fprintf('When system observes (phi), it is Observable and the rank of N_Transpose is\n') 36 | disp(r_N_T_Yphi) 37 | 38 | muo_d = [-5 -5 -5 -5]; 39 | Ko_T_Yphi = acker(A_Linear',C_Yphi',muo_d); 40 | Ko_Yphi = Ko_T_Yphi'; 41 | 42 | fprintf('The Observer Gain "Ko" When System Observes (phi) is\n') 43 | disp(Ko_Yphi) 44 | else 45 | disp('The system is unobservable when it observes (phi)') 46 | end 47 | 48 | %% Simulation 49 | T = 30; 50 | dt = 0.001; 51 | X0 = [0.2;0;0.174533;0]; 52 | Xh0 = [0.01;0.01;0.1;0.1]; 53 | t = 0; 54 | X(:,1) = X0; 55 | Xh(:,1) = Xh0; 56 | Time(1) = t; 57 | k = 1; 58 | while t < T 59 | Xj = X(:,k); 60 | Xhj = Xh(:,k); 61 | y = C_Yphi*Xj; 62 | u = -K_Reg*Xhj; 63 | D1 = Beam_Ball_Regul_FO_Proj(t,Xj,u); 64 | D2 = Beam_Ball_Regul_FO_Proj(t+dt/2,Xj+D1*dt/2,u); 65 | D3 = Beam_Ball_Regul_FO_Proj(t+dt/2,Xj+D2*dt/2,u); 66 | D4 = Beam_Ball_Regul_FO_Proj(t+dt,Xj+D3*dt,u); 67 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 68 | X(:,k+1) = Xj; 69 | O1 = Beam_Ball_Phi_Full_Obser_Regual_Proj(t,Xhj,u,y); 70 | O2 = Beam_Ball_Phi_Full_Obser_Regual_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 71 | O3 = Beam_Ball_Phi_Full_Obser_Regual_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 72 | O4 = Beam_Ball_Phi_Full_Obser_Regual_Proj(t+dt,Xhj+O3*dt,u,y); 73 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 74 | Xh(:,k+1) = Xhj; 75 | Time(k+1) = t + dt; 76 | k = k + 1; 77 | t = t + dt; 78 | end 79 | 80 | %% Plots 81 | figure; 82 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g'); 83 | title('Regulator With Full Order Observer "Phi" Controller Design Initial Conddition "0.2,0,10,0"') 84 | xlabel('time(s)') 85 | ylabel('x(m)') 86 | legend('X','Xhat','location','northeast') 87 | 88 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(1,:),'g'); 89 | xlabel('time(s)') 90 | ylabel('xdot(m/s)') 91 | legend('X','Xhat','location','northeast') 92 | 93 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(1,:),'g'); 94 | xlabel('time(s)') 95 | ylabel('phi(rad)') 96 | legend('X','Xhat','location','northeast') 97 | 98 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(1,:),'g'); 99 | xlabel('time(s)') 100 | ylabel('phidot(rad/s)') 101 | legend('X','Xhat','location','northeast') 102 | xlabel('time(s)') 103 | ylabel('phidot(rad/s)') -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator__x_Full_Observer_Design/Beam_Ball_Regul_FO_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Regul_FO_Proj(t,X,u) 2 | 3 | global A B m g J_Beam 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator__x_Full_Observer_Design/Beam_Ball_x_Full_Obser_Regual_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Beam_Ball_x_Full_Obser_Regual_Proj(t,Xh,u,y) 2 | global A B C_Yx Ko_Yx 3 | 4 | yh = C_Yx*Xh; 5 | DXh = A*Xh + B*u + Ko_Yx*(y-yh); -------------------------------------------------------------------------------- /Ball_Beam_System_Regulator__x_Full_Observer_Design/Project_Beam_Ball_System_Regulator_x_Full_Observer_Desig.m: -------------------------------------------------------------------------------- 1 | %% %% Project#1_Advanced_Control_Beam_Ball_System_Regulator_x_Full_Observer_Design 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx Ko_Yx 6 | %% Designing Controller Gain, Ackerman Method 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | 15 | 16 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 17 | r_M = rank(M); 18 | if r_M == min(size(M)) 19 | fprintf('The system is controllable and the rank of M is\n') 20 | disp(r_M) 21 | 22 | mu_d = [-3 -4 -2+1i -2-1i]; %% Desired Eigenvalues 23 | K_Reg = acker(A_Linear,B_Linear,mu_d); 24 | 25 | fprintf('The Controller Gain "K" is\n') 26 | disp(K_Reg) 27 | else 28 | disp('The System is Unctrollable') 29 | end 30 | 31 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 32 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 33 | r_N_T_Yx = rank(N_T_Yx); 34 | if r_N_T_Yx == min(size(N_T_Yx)) 35 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 36 | disp(r_N_T_Yx) 37 | 38 | muo_d = [-5 -5 -5 -5]; 39 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 40 | Ko_Yx = Ko_T_Yx'; 41 | 42 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 43 | disp(Ko_Yx) 44 | else 45 | disp('The system is unobservable when it observes (x)') 46 | end 47 | 48 | %% Simulation 49 | T = 30; 50 | dt = 0.001; 51 | X0 = [0.2;0;0.174533;0]; 52 | Xh0 = [0.01;0.01;0.01;0.01]; 53 | t = 0; 54 | X(:,1) = X0; 55 | Xh(:,1) = Xh0; 56 | Time(1) = t; 57 | k = 1; 58 | while t < T 59 | Xj = X(:,k); 60 | Xhj = Xh(:,k); 61 | y = C_Yx*Xj; 62 | u = -K_Reg*Xhj; 63 | D1 = Beam_Ball_Regul_FO_Proj(t,Xj,u); 64 | D2 = Beam_Ball_Regul_FO_Proj(t+dt/2,Xj+D1*dt/2,u); 65 | D3 = Beam_Ball_Regul_FO_Proj(t+dt/2,Xj+D2*dt/2,u); 66 | D4 = Beam_Ball_Regul_FO_Proj(t+dt,Xj+D3*dt,u); 67 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 68 | X(:,k+1) = Xj; 69 | O1 = Beam_Ball_x_Full_Obser_Regual_Proj(t,Xhj,u,y); 70 | O2 = Beam_Ball_x_Full_Obser_Regual_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 71 | O3 = Beam_Ball_x_Full_Obser_Regual_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 72 | O4 = Beam_Ball_x_Full_Obser_Regual_Proj(t+dt,Xhj+O3*dt,u,y); 73 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 74 | Xh(:,k+1) = Xhj; 75 | Time(k+1) = t + dt; 76 | k = k + 1; 77 | t = t + dt; 78 | end 79 | 80 | %% Plots 81 | figure; 82 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g'); 83 | title('Regulator With Full Order Observer "x" Controller Design Initial Conddition "0.2,0,10,0"') 84 | xlabel('time(s)') 85 | ylabel('x(m)') 86 | legend('X','Xhat','location','northeast') 87 | 88 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(1,:),'g'); 89 | xlabel('time(s)') 90 | ylabel('xdot(m/s)') 91 | legend('X','Xhat','location','northeast') 92 | 93 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(1,:),'g'); 94 | xlabel('time(s)') 95 | ylabel('phi(rad)') 96 | legend('X','Xhat','location','northeast') 97 | 98 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(1,:),'g'); 99 | xlabel('time(s)') 100 | ylabel('phidot(rad/s)') 101 | legend('X','Xhat','location','northeast') 102 | xlabel('time(s)') 103 | ylabel('phidot(rad/s)') -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator/Beam_Ball_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Servo_Add_Int_Proj(t,X,u,yr) 2 | global J_Beam m g C_Yx A B 3 | 4 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 5 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 6 | 7 | Xx = X(1:4); 8 | Xi = X(5); 9 | y = C_Yx*Xx; 10 | 11 | DXx = A*Xx + B*u; 12 | DXi = yr - y; 13 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator/Project_Beam_Ball_System_Servo_Add_Int_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Beam_Ball_System_Servo_Adding_Integrator 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | 15 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 16 | Bh_Linear = [B_Linear;0]; 17 | 18 | %% Designing Controller Gain, Ackerman Method 19 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear]; 20 | r_Mh = rank(Mh); 21 | if r_Mh == min(size(Mh)) 22 | fprintf('The system is controllable and the rank of Mh is\n') 23 | disp(r_Mh) 24 | 25 | mu_d = [-2+1i -2-1i -3 -3 -4]; %% Desired Eigenvalues 26 | Kh_srv = acker(Ah_Linear,Bh_Linear,mu_d); 27 | 28 | fprintf('The Controller Gain "K" is\n') 29 | disp(Kh_srv) 30 | else 31 | disp('The System is Unctrollable') 32 | end 33 | 34 | %% Simulation 35 | T = 60; 36 | dt = 0.01; 37 | X0 = [0.2;0;0.174533;0;0]; 38 | t = 0; 39 | X(:,1) = X0; 40 | Time(1) = t; 41 | k = 1; 42 | while t < T 43 | Xj = X(:,k); 44 | yr(k) = 0.1*sign(sin(0.2*t)); 45 | u = -Kh_srv*Xj; 46 | D1 = Beam_Ball_Servo_Add_Int_Proj(t,Xj,u,yr(k)); 47 | D2 = Beam_Ball_Servo_Add_Int_Proj(t+dt/2,Xj+D1*dt/2,u,yr(k)); 48 | D3 = Beam_Ball_Servo_Add_Int_Proj(t+dt/2,Xj+D2*dt/2,u,yr(k)); 49 | D4 = Beam_Ball_Servo_Add_Int_Proj(t+dt,Xj+D3*dt,u,yr(k)); 50 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 51 | X(:,k+1) = Xj; 52 | Time(k+1) = t + dt; 53 | k = k + 1; 54 | t = t + dt; 55 | end 56 | 57 | %% Plots 58 | figure; 59 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'g'); 60 | title('Adding Integrator Servo Design Initial Conddition "0.2,0,10,0"') 61 | xlabel('time(s)') 62 | ylabel('x(m)') 63 | legend('Y','Yr','location','northeast') 64 | 65 | subplot(4,1,2);plot(Time,X(2,:)); 66 | xlabel('time(s)') 67 | ylabel('xdot(m/s)') 68 | 69 | subplot(4,1,3);plot(Time,X(3,:)); 70 | xlabel('time(s)') 71 | ylabel('phi(rad)') 72 | 73 | subplot(4,1,4);plot(Time,X(4,:)); 74 | xlabel('time(s)') 75 | ylabel('phidot(rad/s)') -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator_Full_Order_Observer/Beam_Ball_Full_Obser_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Beam_Ball_Full_Obser_Servo_Add_Int_Proj(t,Xh,u,y) 2 | global Ko_Yx A B C_Yx 3 | 4 | yh = C_Yx*Xh; 5 | DXh = A*Xh + B*u + Ko_Yx*(y-yh); -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator_Full_Order_Observer/Beam_Ball_Servo_Add_Int_FO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Servo_Add_Int_FO_Obs_Proj(t,X,u,yr) 2 | global C_Yx A B g J_Beam m 3 | 4 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 5 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 6 | 7 | Xx = X(1:4); 8 | Xi = X(5); 9 | y = C_Yx*Xx; 10 | 11 | DXx = A*Xx + B*u; 12 | DXi = yr - y; 13 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator_Full_Order_Observer/Project_Beam_Ball_System_Servo_Adding_Integrator_FO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Beam_Ball_System_Servo_Adding_Integrator_Full_Order_Observer 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | 15 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 16 | Bh_Linear = [B_Linear;0]; 17 | 18 | %% Designing Controller Gain, Ackerman Method 19 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear]; 20 | r_Mh = rank(Mh); 21 | if r_Mh == min(size(Mh)) 22 | fprintf('The system is controllable and the rank of Mh is\n') 23 | disp(r_Mh) 24 | 25 | mu_d = [-2+1i -2-1i -3 -3 -4]; %% Desired Eigenvalues 26 | Kh_srv = acker(Ah_Linear,Bh_Linear,mu_d); 27 | 28 | fprintf('The Controller Gain "K" is\n') 29 | disp(Kh_srv) 30 | else 31 | disp('The System is Unctrollable') 32 | end 33 | 34 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 35 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 36 | r_N_T_Yx = rank(N_T_Yx); 37 | if r_N_T_Yx == min(size(N_T_Yx)) 38 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 39 | disp(r_N_T_Yx) 40 | 41 | muo_d = [-5 -5 -5 -5]; 42 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 43 | Ko_Yx = Ko_T_Yx'; 44 | 45 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 46 | disp(Ko_Yx) 47 | else 48 | disp('The system is unobservable when it observes (x)') 49 | end 50 | 51 | %% Simulation 52 | T = 60; 53 | dt = 0.001; 54 | X0 = [0.2;0;0.174533;0;0]; 55 | Xh0= [0.01;0.01;0.1;0.1]; 56 | t = 0; 57 | X(:,1) = X0; 58 | Xh(:,1) = Xh0; 59 | Time(1) = t; 60 | k = 1; 61 | while t < T 62 | Xj = X(:,k); 63 | Xhj = Xh(:,k); 64 | y = C_Yx*Xj(1:4); 65 | yr(k) = 0.1*sign(sin(0.2*t)); 66 | u = -Kh_srv(1:4)*Xhj - Kh_srv(5)*Xj(5); 67 | D1 = Beam_Ball_Servo_Add_Int_FO_Obs_Proj(t,Xj,u,yr(k)); 68 | D2 = Beam_Ball_Servo_Add_Int_FO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u,yr(k)); 69 | D3 = Beam_Ball_Servo_Add_Int_FO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u,yr(k)); 70 | D4 = Beam_Ball_Servo_Add_Int_FO_Obs_Proj(t+dt,Xj+D3*dt,u,yr(k)); 71 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 72 | X(:,k+1) = Xj; 73 | O1 = Beam_Ball_Full_Obser_Servo_Add_Int_Proj(t,Xhj,u,y); 74 | O2 = Beam_Ball_Full_Obser_Servo_Add_Int_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 75 | O3 = Beam_Ball_Full_Obser_Servo_Add_Int_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 76 | O4 = Beam_Ball_Full_Obser_Servo_Add_Int_Proj(t+dt,Xhj+O3*dt,u,y); 77 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 78 | Xh(:,k+1) = Xhj; 79 | 80 | Time(k+1) = t+dt; 81 | k = k+1; 82 | t = t + dt; 83 | end 84 | 85 | %% Plots 86 | figure; 87 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g',Time(1:end-1),yr,'r'); 88 | title('Integrator Servo Design with Observer Initial Conddition "0.2,0,10,0"') 89 | xlabel('time(s)') 90 | ylabel('x(m)') 91 | legend('X','Xh','Yr','location','northeast') 92 | 93 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(2,:),'g'); 94 | xlabel('time(s)') 95 | ylabel('xdot(m/s)') 96 | legend('X','Xh','location','northeast') 97 | 98 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(3,:),'g'); 99 | xlabel('time(s)') 100 | ylabel('phi(rad)') 101 | legend('X','Xh','location','northeast') 102 | 103 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(4,:),'g'); 104 | xlabel('time(s)') 105 | ylabel('phidot(rad/s)') 106 | legend('X','Xh','location','northeast') -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator_Minimum_Order_Observer/Beam_Ball_Min_Obser_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DEth=Beam_Ball_Min_Obser_Servo_Add_Int_Proj(t,Eth,u,y) 2 | global Aaa Aab Aba Abb Ba Bb Ko_Yx 3 | 4 | DEth=Abb*Eth + Abb*Ko_Yx*y + Aba*y + Bb*u + Ko_Yx*(-Aaa*y - Aab*Eth - Aab*Ko_Yx*y - Ba*u); -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator_Minimum_Order_Observer/Beam_Ball_Servo_Add_Int_MO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Servo_Add_Int_MO_Obs_Proj(t,X,u,yr) 2 | global C_Yx A B Aaa Aab Aba Abb Ba Bb g m J_Beam 3 | 4 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 5 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 6 | Aaa = A(1); 7 | Aab = A(1,2:4); 8 | Aba = A(2:4,1); 9 | Abb = [A(2,2:4);A(3,2:4);A(4,2:4)]; 10 | Ba = B(1); 11 | Bb = B(2:4,1); 12 | 13 | Xx = X(1:4); 14 | Xi = X(5); 15 | y = C_Yx*Xx; 16 | 17 | DXx = A*Xx + B*u; 18 | DXi = yr - y; 19 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Adding_Integrator_Minimum_Order_Observer/Project_Beam_Ball_System_Servo_Adding_Integrator_MO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Beam_Ball_System_Servo_Adding_Integrator_Minimum_Order_Observer 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx Ko_Yx 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 15 | Bh_Linear = [B_Linear;0]; 16 | Aaa_Linear = A_Linear(1); 17 | Aab_Linear = A_Linear(1,2:4); 18 | Aba_Linear = A_Linear(2:4,1); 19 | Abb_Linear = [A_Linear(2,2:4);A_Linear(3,2:4);A_Linear(4,2:4)]; 20 | Ba_Linear = B_Linear(1); 21 | Bb_Linear = B_Linear(2:4,1); 22 | %% Designing Controller Gain, Ackerman Method 23 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear]; 24 | r_Mh = rank(Mh); 25 | if r_Mh == min(size(Mh)) 26 | fprintf('The system is controllable and the rank of Mh is\n') 27 | disp(r_Mh) 28 | 29 | mu_d = [-2+1i -2-1i -3 -3 -4]; %% Desired Eigenvalues 30 | Kh_srv = acker(Ah_Linear,Bh_Linear,mu_d); 31 | 32 | fprintf('The Controller Gain "K" is\n') 33 | disp(Kh_srv) 34 | else 35 | disp('The System is Unctrollable') 36 | end 37 | 38 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 39 | N_T_Yx = [Aab_Linear' Abb_Linear'*Aab_Linear' (Abb_Linear')^2*Aab_Linear']; %% N tranpose matrix when system observs X1=x 40 | r_N_T_Yx = rank(N_T_Yx); 41 | if r_N_T_Yx == min(size(N_T_Yx)) 42 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 43 | disp(r_N_T_Yx) 44 | 45 | muo_d = [-5 -5 -5]; 46 | Ko_T_Yx = acker(Abb_Linear',Aab_Linear',muo_d); 47 | Ko_Yx = Ko_T_Yx'; 48 | 49 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 50 | disp(Ko_Yx) 51 | else 52 | disp('The system is unobservable when it observes (x)') 53 | end 54 | 55 | %% Simulation 56 | T = 60; 57 | dt = 0.0001; 58 | X0 = [0.2;0;0.174533;0;0]; 59 | Xbh0 = [0.01;0.1;0.1]; 60 | Eth0 = Xbh0 - Ko_Yx*X0(1); 61 | t = 0; 62 | X(:,1) = X0; 63 | Xbh(:,1) = Xbh0; 64 | Eth(:,1) = Eth0; 65 | Time(1) = t; 66 | k = 1; 67 | while t < T 68 | Xj = X(:,k); 69 | Ethj = Eth(:,k); 70 | Xbhj = Ethj + Ko_Yx*Xj(1); 71 | y = C_Yx*Xj(1:4); %%%% y=Xa=X1; 72 | yr(k) = 0.1*sign(sin(0.2*t)); 73 | u = -Kh_srv(1)*Xj(1) - Kh_srv(2:4)*Xbhj - Kh_srv(5)*Xj(5); 74 | D1 = Beam_Ball_Servo_Add_Int_MO_Obs_Proj(t,Xj,u,yr(k)); 75 | D2 = Beam_Ball_Servo_Add_Int_MO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u,yr(k)); 76 | D3 = Beam_Ball_Servo_Add_Int_MO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u,yr(k)); 77 | D4 = Beam_Ball_Servo_Add_Int_MO_Obs_Proj(t+dt,Xj+D3*dt,u,yr(k)); 78 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 79 | X(:,k+1) = Xj; 80 | O1 = Beam_Ball_Min_Obser_Servo_Add_Int_Proj(t,Ethj,u,y); 81 | O2 = Beam_Ball_Min_Obser_Servo_Add_Int_Proj(t+dt/2,Ethj+O1*dt/2,u,y); 82 | O3 = Beam_Ball_Min_Obser_Servo_Add_Int_Proj(t+dt/2,Ethj+O2*dt/2,u,y); 83 | O4 = Beam_Ball_Min_Obser_Servo_Add_Int_Proj(t+dt,Ethj+O3*dt,u,y); 84 | Ethj = Ethj+(O1+2*O2+2*O3+O4)/6*dt; 85 | Eth(:,k+1) = Ethj; 86 | Xbh(:,k+1) = Ethj + Ko_Yx*Xj(1); 87 | Time(k+1) = t + dt; 88 | k = k + 1; 89 | t = t + dt; 90 | end 91 | 92 | %% Plots 93 | figure; 94 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'r'); 95 | title('Integrator Servo Design with Minimum Order Observer Initial Conddition "0.2,0,10,0"') 96 | xlabel('time(s)') 97 | ylabel('x(m)') 98 | legend('X','Yr','location','northeast') 99 | 100 | subplot(4,1,2);plot(Time,X(2,:),Time,Xbh(1,:),'g'); 101 | xlabel('time(s)') 102 | ylabel('xdot(m/s)') 103 | legend('X','Xbh','location','northeast') 104 | 105 | subplot(4,1,3);plot(Time,X(3,:),Time,Xbh(2,:),'g'); 106 | xlabel('time(s)') 107 | ylabel('phi(rad)') 108 | legend('X','Xbh','location','northeast') 109 | 110 | subplot(4,1,4);plot(Time,X(4,:),Time,Xbh(3,:),'g'); 111 | xlabel('time(s)') 112 | ylabel('phidot(rad/s)') 113 | legend('X','Xbh','location','northeast') -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward/Beam_Ball_Servo_FF_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Servo_FF_Proj(t,X,u) 2 | 3 | global J_Beam m g A B 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward/Project_Beam_Ball_System_Servo_Feed_Forward.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Beam_Ball_System_Servo_Feed_Forward 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx A B 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | %% Designing Controller Gain, Ackerman Method 15 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 16 | r_M = rank(M); 17 | if r_M == min(size(M)) 18 | fprintf('The system is controllable and the rank of M is\n') 19 | disp(r_M) 20 | 21 | mu_d = [-3 -4 -2+1i -2-1i]; %% Desired Eigenvalues 22 | K_srv = acker(A_Linear,B_Linear,mu_d); 23 | 24 | fprintf('The Controller Gain "K" is\n') 25 | disp(K_srv) 26 | else 27 | disp('The System is Unctrollable') 28 | end 29 | 30 | 31 | %% Simulation 32 | T = 60; 33 | dt = 0.01; 34 | X0 = [0.2;0;0.174533;0]; 35 | t = 0; 36 | X(:,1) = X0; 37 | Time(1) = t; 38 | k = 1; 39 | while t < T 40 | Xj = X(:,k); 41 | y = C_Yx*Xj; 42 | yr(k) = 0.1*sign(sin(0.2*t)); 43 | uff = (-yr(k))/(C_Yx*inv(A-B*K_srv)*B); 44 | u = -K_srv*Xj + uff; 45 | D1 = Beam_Ball_Servo_FF_Proj(t,Xj,u); 46 | D2 = Beam_Ball_Servo_FF_Proj(t+dt/2,Xj+D1*dt/2,u); 47 | D3 = Beam_Ball_Servo_FF_Proj(t+dt/2,Xj+D2*dt/2,u); 48 | D4 = Beam_Ball_Servo_FF_Proj(t+dt,Xj+D3*dt,u); 49 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 50 | X(:,k+1) = Xj; 51 | 52 | Time(k+1) = t + dt; 53 | k = k + 1; 54 | t = t + dt; 55 | end 56 | 57 | %% Plots 58 | figure; 59 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'g'); 60 | title('Feed Forward Servo Controller Design Initial Conddition "0.2,0,10,0"') 61 | xlabel('time(s)') 62 | ylabel('x(m)') 63 | legend('Y','Yr','location','northeast') 64 | 65 | subplot(4,1,2);plot(Time,X(2,:)); 66 | xlabel('time(s)') 67 | ylabel('xdot(m/s)') 68 | 69 | subplot(4,1,3);plot(Time,X(3,:)); 70 | xlabel('time(s)') 71 | ylabel('phi(rad)') 72 | 73 | subplot(4,1,4);plot(Time,X(4,:)); 74 | xlabel('time(s)') 75 | ylabel('phidot(rad/s)') -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward_Full_Order_Observer/Beam_Ball_Full_Obser_Servo_FF_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Beam_Ball_Full_Obser_Servo_FF_Proj(t,Xh,u,y) 2 | global Ko_Yx A B C_Yx 3 | 4 | yh = C_Yx*Xh; 5 | DXh = A*Xh + B*u + Ko_Yx*(y-yh); -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward_Full_Order_Observer/Beam_Ball_Servo_FF_FO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Servo_FF_FO_Obs_Proj(t,X,u) 2 | 3 | global J_Beam m g A B 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward_Full_Order_Observer/Project_Beam_Ball_System_Servo_FF_FO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Beam_Ball_System_Servo_Feed_Forward_Full_Order_Observer 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx A B 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | 15 | %% Designing Controller Gain, Ackerman Method 16 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 17 | r_M = rank(M); 18 | if r_M == min(size(M)) 19 | fprintf('The system is controllable and the rank of M is\n') 20 | disp(r_M) 21 | 22 | mu_d = [-3 -4 -2+1i -2-1i]; %% Desired Eigenvalues 23 | K_srv = acker(A_Linear,B_Linear,mu_d); 24 | 25 | fprintf('The Controller Gain "K" is\n') 26 | disp(K_srv) 27 | else 28 | disp('The System is Unctrollable') 29 | end 30 | 31 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 32 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 33 | r_N_T_Yx = rank(N_T_Yx); 34 | if r_N_T_Yx == min(size(N_T_Yx)) 35 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 36 | disp(r_N_T_Yx) 37 | 38 | muo_d = [-5 -5 -5 -5]; 39 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 40 | Ko_Yx = Ko_T_Yx'; 41 | 42 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 43 | disp(Ko_Yx) 44 | else 45 | disp('The system is unobservable when it observes (x)') 46 | end 47 | 48 | %% Simulation 49 | T = 60; 50 | dt = 0.001; 51 | X0 = [0.2;0;0.174533;0]; 52 | Xh0 = [0.01;0.01;0.1;0.1]; 53 | t = 0; 54 | X(:,1) = X0; 55 | Xh(:,1) = Xh0; 56 | Time(1) = t; 57 | k = 1; 58 | while t < T 59 | Xj = X(:,k); 60 | Xhj = Xh(:,k); 61 | y = C_Yx*Xj; 62 | yr(k) = 0.1*sign(sin(0.2*t)); 63 | uff = (-yr(k))/(C_Yx*inv(A-B*K_srv)*B); 64 | u = -K_srv*Xj + uff; 65 | D1 = Beam_Ball_Servo_FF_FO_Obs_Proj(t,Xj,u); 66 | D2 = Beam_Ball_Servo_FF_FO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u); 67 | D3 = Beam_Ball_Servo_FF_FO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u); 68 | D4 = Beam_Ball_Servo_FF_FO_Obs_Proj(t+dt,Xj+D3*dt,u); 69 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 70 | X(:,k+1) = Xj; 71 | O1 = Beam_Ball_Full_Obser_Servo_FF_Proj(t,Xhj,u,y); 72 | O2 = Beam_Ball_Full_Obser_Servo_FF_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 73 | O3 = Beam_Ball_Full_Obser_Servo_FF_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 74 | O4 = Beam_Ball_Full_Obser_Servo_FF_Proj(t+dt,Xhj+O3*dt,u,y); 75 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 76 | Xh(:,k+1) = Xhj; 77 | 78 | Time(k+1) = t+dt; 79 | k = k+1; 80 | t = t + dt; 81 | end 82 | 83 | %% Plots 84 | figure; 85 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g',Time(1:end-1),yr,'r'); 86 | title('Feed Forward Servo Design with Observer Initial Conddition "0.2,0,10,0"') 87 | xlabel('time(s)') 88 | ylabel('x(m)') 89 | legend('X','Xh','Yr','location','northeast') 90 | 91 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(2,:),'g'); 92 | xlabel('time(s)') 93 | ylabel('xdot(m/s)') 94 | legend('X','Xh','location','northeast') 95 | 96 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(3,:),'g'); 97 | xlabel('time(s)') 98 | ylabel('phi(rad)') 99 | legend('X','Xh','location','northeast') 100 | 101 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(4,:),'g'); 102 | xlabel('time(s)') 103 | ylabel('phidot(rad/s)') 104 | legend('X','Xh','location','northeast') -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward_Minimum_Order_Observer/Beam_Ball_Min_Obser_Servo_FF_Proj.m: -------------------------------------------------------------------------------- 1 | function DEth=Beam_Ball_Min_Obser_Servo_FF_Proj(t,Eth,u,y) 2 | global Aaa Aab Aba Abb Ba Bb Ko_Yx 3 | 4 | DEth=Abb*Eth + Abb*Ko_Yx*y + Aba*y + Bb*u + Ko_Yx*(-Aaa*y - Aab*Eth - Aab*Ko_Yx*y - Ba*u); -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward_Minimum_Order_Observer/Beam_Ball_Servo_FF_MO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Beam_Ball_Servo_FF_MO_Obs_Proj(t,X,u) 2 | 3 | global A B Aaa Aab Aba Abb Ba Bb g m J_Beam 4 | 5 | A = [0 1 0 0;0 0 (-g*sin(X(3)))/(1.4*X(3)) (X(1)*X(4))/(1.4);0 0 0 1;(m*g*cos(X(3)))/(m*X(1)^2+J_Beam) 0 0 (-2*m*X(1)*X(2))/(m*X(1)^2+J_Beam)]; 6 | B = [0;0;0;1/(m*X(1)^2+J_Beam)]; 7 | Aaa = A(1); 8 | Aab = A(1,2:4); 9 | Aba = A(2:4,1); 10 | Abb = [A(2,2:4);A(3,2:4);A(4,2:4)]; 11 | Ba = B(1); 12 | Bb = B(2:4,1); 13 | 14 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Ball_Beam_System_Servo_Feed_Forward_Minimum_Order_Observer/Project_Beam_Ball_System_Servo_FF_MO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Beam_Ball_System_Servo_Feed_Forward_Minimum_Order_Observer 2 | clc 3 | clear 4 | 5 | global m g J_Beam C_Yx A B Ko_Yx 6 | %% System Parameters 7 | m = 0.5; %% Disk Mass 8 | g = 9.81; 9 | J_Beam = 2; 10 | 11 | A_Linear = [0 1 0 0;0 0 (-g)/(1.4) 0;0 0 0 1;(-m*g)/J_Beam 0 0 0]; 12 | B_Linear = [0;0;0;1/J_Beam]; 13 | C_Yx = [1 0 0 0]; 14 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 15 | Bh_Linear = [B_Linear;0]; 16 | Aaa_Linear = A_Linear(1); 17 | Aab_Linear = A_Linear(1,2:4); 18 | Aba_Linear = A_Linear(2:4,1); 19 | Abb_Linear = [A_Linear(2,2:4);A_Linear(3,2:4);A_Linear(4,2:4)]; 20 | Ba_Linear = B_Linear(1); 21 | Bb_Linear = B_Linear(2:4,1); 22 | 23 | %% Designing Controller Gain, Ackerman Method 24 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 25 | r_M = rank(M); 26 | if r_M == min(size(M)) 27 | fprintf('The system is controllable and the rank of M is\n') 28 | disp(r_M) 29 | 30 | mu_d = [-3 -4 -2+1i -2-1i]; %% Desired Eigenvalues 31 | K_srv = acker(A_Linear,B_Linear,mu_d); 32 | 33 | fprintf('The Controller Gain "K" is\n') 34 | disp(K_srv) 35 | else 36 | disp('The System is Unctrollable') 37 | end 38 | 39 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 40 | N_T_Yx = [Aab_Linear' Abb_Linear'*Aab_Linear' (Abb_Linear')^2*Aab_Linear']; %% N tranpose matrix when system observs X1=x 41 | r_N_T_Yx = rank(N_T_Yx); 42 | if r_N_T_Yx == min(size(N_T_Yx)) 43 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 44 | disp(r_N_T_Yx) 45 | 46 | muo_d = [-5 -5 -5]; 47 | Ko_T_Yx = acker(Abb_Linear',Aab_Linear',muo_d); 48 | Ko_Yx = Ko_T_Yx'; 49 | 50 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 51 | disp(Ko_Yx) 52 | else 53 | disp('The system is unobservable when it observes (x)') 54 | end 55 | 56 | %% Simulation 57 | T = 60; 58 | dt = 0.0001; 59 | X0 = [0.2;0;0.174533;0]; 60 | Xbh0 = [0.01;0.1;0.1]; 61 | Eth0 = Xbh0 - Ko_Yx*X0(1); 62 | t = 0; 63 | X(:,1) = X0; 64 | Xbh(:,1) = Xbh0; 65 | Eth(:,1) = Eth0; 66 | Time(1) = t; 67 | k = 1; 68 | while t < T 69 | Xj = X(:,k); 70 | Ethj = Eth(:,k); 71 | Xbhj = Ethj + Ko_Yx*Xj(1); 72 | y = C_Yx*Xj(1:4); %%%% y=Xa=X1; 73 | yr(k) = 0.1*sign(sin(0.2*t)); 74 | uff = -yr(k)/(C_Yx*inv(A-B*K_srv)*B); 75 | u = -K_srv(1)*Xj(1) - K_srv(2:4)*Xbhj + uff; 76 | D1 = Beam_Ball_Servo_FF_MO_Obs_Proj(t,Xj,u); 77 | D2 = Beam_Ball_Servo_FF_MO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u); 78 | D3 = Beam_Ball_Servo_FF_MO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u); 79 | D4 = Beam_Ball_Servo_FF_MO_Obs_Proj(t+dt,Xj+D3*dt,u); 80 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 81 | X(:,k+1) = Xj; 82 | O1 = Beam_Ball_Min_Obser_Servo_FF_Proj(t,Ethj,u,y); 83 | O2 = Beam_Ball_Min_Obser_Servo_FF_Proj(t+dt/2,Ethj+O1*dt/2,u,y); 84 | O3 = Beam_Ball_Min_Obser_Servo_FF_Proj(t+dt/2,Ethj+O2*dt/2,u,y); 85 | O4 = Beam_Ball_Min_Obser_Servo_FF_Proj(t+dt,Ethj+O3*dt,u,y); 86 | Ethj = Ethj+(O1+2*O2+2*O3+O4)/6*dt; 87 | Eth(:,k+1) = Ethj; 88 | Xbh(:,k+1) = Ethj + Ko_Yx*Xj(1); 89 | Time(k+1) = t + dt; 90 | k = k + 1; 91 | t = t + dt; 92 | end 93 | 94 | %% Plots 95 | figure; 96 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'r'); 97 | title('Feed Forward Servo Design with Minimum Order Observer Initial Conddition "0.2,0,10,0"') 98 | xlabel('time(s)') 99 | ylabel('x(m)') 100 | legend('X','Yr','location','northeast') 101 | 102 | subplot(4,1,2);plot(Time,X(2,:),Time,Xbh(1,:),'g'); 103 | xlabel('time(s)') 104 | ylabel('xdot(m/s)') 105 | legend('X','Xbh','location','northeast') 106 | 107 | subplot(4,1,3);plot(Time,X(3,:),Time,Xbh(2,:),'g'); 108 | xlabel('time(s)') 109 | ylabel('phi(rad)') 110 | legend('X','Xbh','location','northeast') 111 | 112 | subplot(4,1,4);plot(Time,X(4,:),Time,Xbh(3,:),'g'); 113 | xlabel('time(s)') 114 | ylabel('phidot(rad/s)') 115 | legend('X','Xbh','location','northeast') -------------------------------------------------------------------------------- /CLOS_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#2 2 | ro = 1.225; %kg/m^3% 3 | S = 0.1; %m^2% 4 | g = 9.81; %m/s^2% 5 | aty = 0.5*g; %m/s^2% 6 | atz = 0.5*g; %m/s^2% 7 | Gz = 2; %Constant Guidance Coefficient Elevation Channel% 8 | Gy = 2; %Constant Guidance Coefficient Azimuth Channel% 9 | % A = 0.07; 10 | % B = A*15; 11 | gain = 9.07; 12 | %% Transfer_Function_of_Lead-Lag_Compensator 13 | num_lead_comp = [1 A]; 14 | den_lean_comp = [1 B]; 15 | lead_comp = tf(num_lead_comp,den_lean_comp); 16 | 17 | num_controller = [0 1]; 18 | den_controller = [0.2 1]; 19 | controller = tf(num_controller,den_controller); 20 | 21 | open_loop_tf = lead_comp*controller; 22 | close_loop_tf = feedback(open_loop_tf,1); 23 | 24 | %rlocus(open_loop_tf); -------------------------------------------------------------------------------- /CLOS_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration/Problem_2_Part_C.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/CLOS_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration/Problem_2_Part_C.slx -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Regulator_Design/MIMO_DOuble_Pendulum_Regul_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_DOuble_Pendulum_Regul_Proj(t,X,u) 2 | 3 | DX1 = X(2); 4 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 5 | DX3 = X(4); 6 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 7 | DX5 = X(6); 8 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 9 | DX= [DX1;DX2;DX3;DX4;DX5;DX6]; -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Regulator_Design/MIMO_Inverted_Double_Pendulum_System_Regulator_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Regulator_Design 2 | clc 3 | clear 4 | 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | 11 | %% Controllability and Observability 12 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear A_Linear^4*B_Linear]; 13 | r_M = rank(M); 14 | if r_M == min(size(M)) 15 | 16 | fprintf('The system is controllable and the rank of M is\n') 17 | disp(r_M) 18 | 19 | else 20 | 21 | disp('The System is Uncontrollable') 22 | 23 | end 24 | 25 | N = [C;C*A_Linear;C*A_Linear^2;C*A_Linear^3;C*A_Linear^4;C*A_Linear^5]; 26 | r_N = rank(N); 27 | if r_N == min(size(N)) 28 | 29 | fprintf('The system is observable and the rank of N is\n') 30 | disp(r_N) 31 | 32 | else 33 | 34 | disp('The System is Unobservable') 35 | 36 | end 37 | 38 | %% Designing Controller Gain, EESA Method 39 | mu1 = -4; 40 | mu2 = -4; 41 | mu3 = -3; 42 | mu4 = -3; 43 | mu5 = -2+1i; 44 | mu6 = -2-1i; 45 | Amu1 = [A_Linear-mu1*eye(6) B_Linear]; 46 | Amu2 = [A_Linear-mu2*eye(6) B_Linear]; 47 | Amu3 = [A_Linear-mu3*eye(6) B_Linear]; 48 | Amu4 = [A_Linear-mu4*eye(6) B_Linear]; 49 | Amu5 = [A_Linear-mu5*eye(6) B_Linear]; 50 | Amu6 = [A_Linear-mu6*eye(6) B_Linear]; 51 | 52 | S1 = null(Amu1); 53 | S2 = null(Amu2); 54 | S3 = null(Amu3); 55 | S4 = null(Amu4); 56 | S5 = null(Amu5); 57 | S6 = null(Amu6); 58 | 59 | v1q1 = S1(:,1); % n=6, m=2, v=n*1, q=m*1 60 | v2q2 = S2(:,2); 61 | v3q3 = S3(:,1); 62 | v4q4 = S4(:,2); 63 | v5q5 = real(S5(:,1)); 64 | v6q6 = imag(S5(:,1)); 65 | v1 = v1q1(1:6); q1 = v2q2(7:8); 66 | v2 = v2q2(1:6); q2 = v2q2(7:8); 67 | v3 = v3q3(1:6); q3 = v3q3(7:8); 68 | v4 = v4q4(1:6); q4 = v4q4(7:8); 69 | v5 = v5q5(1:6); q5 = v5q5(7:8); 70 | v6 = v6q6(1:6); q6 = v6q6(7:8); 71 | 72 | v = [v1 v2 v3 v4 v5 v6]; 73 | r_v = rank(v); 74 | 75 | if r_v == min(size(v)) 76 | 77 | fprintf('The matrix v is invertable and its rank is\n') 78 | disp(r_v) 79 | 80 | vi = inv(v); 81 | K = -[q1 q2 q3 q4 q5 q6]*vi; 82 | fprintf('The Controller Gain "K" is\n') 83 | disp(K) 84 | 85 | else 86 | 87 | disp('The v vectors must be selected independable') 88 | 89 | end 90 | 91 | 92 | %% Simulation 93 | T = 10; 94 | dt = 0.001; 95 | X0 = [0;0.2;0.0872665;-0.0523599;0.174533;-0.0872665]; 96 | t = 0; 97 | X(:,1) = X0; 98 | Time(1) = t; 99 | k = 1; 100 | while t < T 101 | Xj = X(:,k); 102 | u = -K*Xj; 103 | D1 = MIMO_DOuble_Pendulum_Regul_Proj(t,Xj,u); 104 | D2 = MIMO_DOuble_Pendulum_Regul_Proj(t+dt/2,Xj+D1*dt/2,u); 105 | D3 = MIMO_DOuble_Pendulum_Regul_Proj(t+dt/2,Xj+D2*dt/2,u); 106 | D4 = MIMO_DOuble_Pendulum_Regul_Proj(t+dt,Xj+D3*dt,u); 107 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 108 | X(:,k+1) = Xj; 109 | Time(k+1) = t + dt; 110 | k = k+1; 111 | t = t + dt; 112 | end 113 | 114 | %% Plots 115 | figure; 116 | subplot(6,1,1);plot(Time,X(1,:)); 117 | title('Regulator Controller Design') 118 | xlabel('time(s)') 119 | ylabel('x(m)') 120 | 121 | subplot(6,1,2);plot(Time,X(2,:)); 122 | xlabel('time(s)') 123 | ylabel('xdot(m/s)') 124 | 125 | subplot(6,1,3);plot(Time,X(3,:)); 126 | xlabel('time(s)') 127 | ylabel('theta1(rad)') 128 | 129 | subplot(6,1,4);plot(Time,X(4,:)); 130 | xlabel('time(s)') 131 | ylabel('theta1dot(rad/s)') 132 | 133 | subplot(6,1,5);plot(Time,X(5,:)); 134 | xlabel('time(s)') 135 | ylabel('theta2(rad)') 136 | 137 | subplot(6,1,6);plot(Time,X(6,:)); 138 | xlabel('time(s)') 139 | ylabel('theta2dot(rad/s)') -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design/MIMO_Double_Pendulum_Obser_Regul_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=MIMO_Double_Pendulum_Obser_Regul_Proj(t,Xh,u,y) 2 | global C Ko 3 | 4 | DXh1 = Xh(2); 5 | DXh2 = (140*(cos(Xh(5)) - cos(Xh(3))*cos(Xh(3) - Xh(5)))*((sin(Xh(3) - Xh(5))*Xh(4)^2)/20 + sin(Xh(5))))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (20*(7*cos(Xh(3)) - 2*cos(Xh(5))*cos(Xh(3) - Xh(5)))*(u(2) + (7*sin(Xh(3)))/2 - (Xh(6)^2*sin(Xh(3) - Xh(5)))/20))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (10*(2*cos(Xh(3) - Xh(5))^2 - 7)*((7*sin(Xh(3))*Xh(4)^2)/20 + (sin(Xh(5))*Xh(6)^2)/10 + u(1)))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119); 6 | DXh3 = Xh(4); 7 | DXh4 = (40*((sin(Xh(3) - Xh(5))*Xh(4)^2)/20 + sin(Xh(5)))*(17*cos(Xh(3) - Xh(5)) - 7*cos(Xh(3))*cos(Xh(5))))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (40*(2*cos(Xh(5))^2 - 17)*(u(2) + (7*sin(Xh(3)))/2 - (Xh(6)^2*sin(Xh(3) - Xh(5)))/20))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (20*(7*cos(Xh(3)) - 2*cos(Xh(5))*cos(Xh(3) - Xh(5)))*((7*sin(Xh(3))*Xh(4)^2)/20 + (sin(Xh(5))*Xh(6)^2)/10 + u(1)))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119); 8 | DXh5 = Xh(6); 9 | DXh6 = (40*(17*cos(Xh(3) - Xh(5)) - 7*cos(Xh(3))*cos(Xh(5)))*(u(2) + (7*sin(Xh(3)))/2 - (Xh(6)^2*sin(Xh(3) - Xh(5)))/20))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (140*(cos(Xh(5)) - cos(Xh(3))*cos(Xh(3) - Xh(5)))*((7*sin(Xh(3))*Xh(4)^2)/20 + (sin(Xh(5))*Xh(6)^2)/10 + u(1)))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (140*((sin(Xh(3) - Xh(5))*Xh(4)^2)/20 + sin(Xh(5)))*(7*cos(Xh(3))^2 - 17))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119); 10 | yh = C*Xh; 11 | DXh = [DXh1;DXh2;DXh3;DXh4;DXh5;DXh6] + Ko*(y-yh); -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design/MIMO_Double_Pendulum_Regul_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_Double_Pendulum_Regul_Obs_Proj(t,X,u) 2 | 3 | DX1 = X(2); 4 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 5 | DX3 = X(4); 6 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 7 | DX5 = X(6); 8 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 9 | DX= [DX1;DX2;DX3;DX4;DX5;DX6]; -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design/MIMO_Inverted_Double_Pendulum_System_Regulator_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Regulator_Observer_Design 2 | clc 3 | clear 4 | global C Ko 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | 11 | %% Controllability and Observability 12 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear A_Linear^4*B_Linear]; 13 | r_M = rank(M); 14 | if r_M == min(size(M)) 15 | 16 | fprintf('The system is controllable and the rank of M is\n') 17 | disp(r_M) 18 | 19 | else 20 | 21 | disp('The System is Uncontrollable') 22 | 23 | end 24 | 25 | N = [C;C*A_Linear;C*A_Linear^2;C*A_Linear^3;C*A_Linear^4;C*A_Linear^5]; 26 | r_N = rank(N); 27 | if r_N == min(size(N)) 28 | 29 | fprintf('The system is observable and the rank of N is\n') 30 | disp(r_N) 31 | 32 | else 33 | 34 | disp('The System is Unobservable') 35 | 36 | end 37 | 38 | %% Designing Controller Gain, EESA Method 39 | mu1 = -4; 40 | mu2 = -4; 41 | mu3 = -3; 42 | mu4 = -3; 43 | mu5 = -2+1i; 44 | mu6 = -2-1i; 45 | Amu1 = [A_Linear-mu1*eye(6) B_Linear]; 46 | Amu2 = [A_Linear-mu2*eye(6) B_Linear]; 47 | Amu3 = [A_Linear-mu3*eye(6) B_Linear]; 48 | Amu4 = [A_Linear-mu4*eye(6) B_Linear]; 49 | Amu5 = [A_Linear-mu5*eye(6) B_Linear]; 50 | Amu6 = [A_Linear-mu6*eye(6) B_Linear]; 51 | 52 | S1 = null(Amu1); 53 | S2 = null(Amu2); 54 | S3 = null(Amu3); 55 | S4 = null(Amu4); 56 | S5 = null(Amu5); 57 | S6 = null(Amu6); 58 | 59 | v1q1 = S1(:,1); % n=6, m=2, v=n*1, q=m*1 60 | v2q2 = S2(:,2); 61 | v3q3 = S3(:,1); 62 | v4q4 = S4(:,2); 63 | v5q5 = real(S5(:,1)); 64 | v6q6 = imag(S5(:,1)); 65 | v1 = v1q1(1:6); q1 = v2q2(7:8); 66 | v2 = v2q2(1:6); q2 = v2q2(7:8); 67 | v3 = v3q3(1:6); q3 = v3q3(7:8); 68 | v4 = v4q4(1:6); q4 = v4q4(7:8); 69 | v5 = v5q5(1:6); q5 = v5q5(7:8); 70 | v6 = v6q6(1:6); q6 = v6q6(7:8); 71 | 72 | v = [v1 v2 v3 v4 v5 v6]; 73 | r_v = rank(v); 74 | 75 | if r_v == min(size(v)) 76 | 77 | fprintf('The matrix v is invertable and its rank is\n') 78 | disp(r_v) 79 | 80 | vi = inv(v); 81 | K = -[q1 q2 q3 q4 q5 q6]*vi; 82 | fprintf('The Controller Gain "K" is\n') 83 | disp(K) 84 | 85 | else 86 | 87 | disp('The v vectors must be selected independable') 88 | 89 | end 90 | 91 | %% Designing Observer Gain, EESA Method 92 | NT = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C' (A_Linear')^4*C' (A_Linear')^5*C']; 93 | r_NT = rank(NT); 94 | if r_NT == min(size(NT)) 95 | 96 | fprintf('The system is observable and the rank of NT is\n') 97 | disp(r_NT) 98 | 99 | else 100 | 101 | disp('The System is Unobservable') 102 | 103 | end 104 | 105 | muo1 = -5; 106 | muo2 = -6; 107 | muo3 = -7; 108 | muo4 = -8; 109 | muo5 = -9; 110 | muo6 = -10; 111 | Amuo1 = [A_Linear'-muo1*eye(6) C']; 112 | Amuo2 = [A_Linear'-muo2*eye(6) C']; 113 | Amuo3 = [A_Linear'-muo3*eye(6) C']; 114 | Amuo4 = [A_Linear'-muo4*eye(6) C']; 115 | Amuo5 = [A_Linear'-muo5*eye(6) C']; 116 | Amuo6 = [A_Linear'-muo6*eye(6) C']; 117 | 118 | S1o = null(Amuo1); 119 | S2o = null(Amuo2); 120 | S3o = null(Amuo3); 121 | S4o = null(Amuo4); 122 | S5o = null(Amuo5); 123 | S6o = null(Amuo6); 124 | 125 | v1q1o = S1o(:,2); % n=6, m=2, v=n*1, q=m*1 126 | v2q2o = S2o(:,1); 127 | v3q3o = S3o(:,1); 128 | v4q4o = S4o(:,1); 129 | v5q5o = S5o(:,2); 130 | v6q6o = S6o(:,2)-S6o(:,1); 131 | v1o = v1q1o(1:6); q1o = v2q2o(7:8); 132 | v2o = v2q2o(1:6); q2o = v2q2o(7:8); 133 | v3o = v3q3o(1:6); q3o = v3q3o(7:8); 134 | v4o = v4q4o(1:6); q4o = v4q4o(7:8); 135 | v5o = v5q5o(1:6); q5o = v5q5o(7:8); 136 | v6o = v6q6o(1:6); q6o = v6q6o(7:8); 137 | 138 | vo = [v1o v2o v3o v4o v5o v6o]; 139 | r_vo = rank(vo); 140 | 141 | if r_vo == min(size(vo)) 142 | 143 | fprintf('The matrix vo is invertable and the its rank is\n') 144 | disp(r_vo) 145 | 146 | vio = inv(vo); 147 | KoT = -[q1o q2o q3o q4o q5o q6o]*vio; 148 | Ko = KoT'; 149 | fprintf('The Observer Gain "Ko" is\n') 150 | disp(Ko) 151 | 152 | else 153 | 154 | disp('The vo vectors must be selected independable') 155 | 156 | end 157 | %% Simulation 158 | T = 10; 159 | dt = 0.001; 160 | X0 = [0;0.2/2;0.0872665/2;-0.0523599/2;0.174533/2;-0.0872665/2]; 161 | Xh0 = [0.005;0.075;0.04/2;-0.07/2;0.6/2;-0.2/2]; 162 | t = 0; 163 | X(:,1) = X0; 164 | Xh(:,1) = Xh0; 165 | Time(1) = t; 166 | k = 1; 167 | while t < T 168 | Xj = X(:,k); 169 | Xhj = Xh(:,k); 170 | y = C*Xj; 171 | u = -K*Xhj; 172 | D1 = MIMO_Double_Pendulum_Regul_Obs_Proj(t,Xj,u); 173 | D2 = MIMO_Double_Pendulum_Regul_Obs_Proj(t+dt/2,Xj+D1*dt/2,u); 174 | D3 = MIMO_Double_Pendulum_Regul_Obs_Proj(t+dt/2,Xj+D2*dt/2,u); 175 | D4 = MIMO_Double_Pendulum_Regul_Obs_Proj(t+dt,Xj+D3*dt,u); 176 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 177 | X(:,k+1) = Xj; 178 | O1 = MIMO_Double_Pendulum_Obser_Regul_Proj(t,Xhj,u,y); 179 | O2 = MIMO_Double_Pendulum_Obser_Regul_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 180 | O3 = MIMO_Double_Pendulum_Obser_Regul_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 181 | O4 = MIMO_Double_Pendulum_Obser_Regul_Proj(t+dt,Xhj+O3*dt,u,y); 182 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 183 | Xh(:,k+1) = Xhj; 184 | Time(k+1) = t + dt; 185 | k = k + 1; 186 | t = t + dt; 187 | end 188 | 189 | %% Plots 190 | figure; 191 | subplot(6,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g'); 192 | title('Regulator With Observer Design ') 193 | xlabel('time(s)') 194 | ylabel('X(m)') 195 | legend('X','Xhat','location','northeast') 196 | 197 | subplot(6,1,2);plot(Time,X(2,:),Time,Xh(2,:),'g'); 198 | xlabel('time(s)') 199 | ylabel('Xdot(m/s)') 200 | legend('X','Xhat','location','northeast') 201 | 202 | subplot(6,1,3);plot(Time,X(3,:),Time,Xh(3,:),'g'); 203 | xlabel('time(s)') 204 | ylabel('theta1(rad)') 205 | legend('X','Xhat','location','northeast') 206 | 207 | subplot(6,1,4);plot(Time,X(4,:),Time,Xh(4,:),'g'); 208 | xlabel('time(s)') 209 | ylabel('theta1dot(rad/s)') 210 | legend('X','Xhat','location','northeast') 211 | 212 | subplot(6,1,5);plot(Time,X(5,:),Time,Xh(5,:),'g'); 213 | xlabel('time(s)') 214 | ylabel('theta2(rad)') 215 | legend('X','Xhat','location','northeast') 216 | 217 | subplot(6,1,6);plot(Time,X(6,:),Time,Xh(6,:),'g'); 218 | xlabel('time(s)') 219 | ylabel('theta2dot(rad/s)') 220 | legend('X','Xhat','location','northeast') -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator/MIMO_Double_Pendulum_Servo_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_Double_Pendulum_Servo_Proj(t,X,u,yr1,yr2) 2 | global C 3 | 4 | Xx=X(1:6); 5 | Xi1=X(7); 6 | Xi2=X(8); 7 | y=C*Xx; 8 | 9 | DX1 = X(2); 10 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 11 | DX3 = X(4); 12 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 13 | DX5 = X(6); 14 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 15 | DXx= [DX1;DX2;DX3;DX4;DX5;DX6]; 16 | 17 | DXi1=yr1-y(1); 18 | DXi2=yr2-y(2); 19 | DX=[DXx;DXi1;DXi2]; -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator/MIMO_Inverted_Double_Pendulum_System_Servo_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Servo_Design 2 | clc 3 | clear 4 | global C 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | Ah_Linear = [A_Linear zeros(6,2);-C zeros(2,2)]; %n=8, m=2 11 | Bh_Linear = [B_Linear;zeros(2,2)]; 12 | %% Controllability and Observability 13 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear Ah_Linear^5*Bh_Linear Ah_Linear^6*Bh_Linear]; 14 | r_Mh = rank(Mh); 15 | if r_Mh == min(size(Mh)) 16 | 17 | fprintf('The system is controllable and the rank of Mh is\n') 18 | disp(r_Mh) 19 | 20 | else 21 | 22 | disp('The System is Uncontrollable') 23 | 24 | end 25 | 26 | N = [C;C*A_Linear;C*A_Linear^2;C*A_Linear^3;C*A_Linear^4;C*A_Linear^5]; 27 | r_N = rank(N); 28 | if r_N == min(size(N)) 29 | 30 | fprintf('The system is observable and the rank of N is\n') 31 | disp(r_N) 32 | 33 | else 34 | 35 | disp('The System is Unobservable') 36 | 37 | end 38 | 39 | %% Designing Controller Gain, EESA Method 40 | mu1 = -4; 41 | mu2 = -4; 42 | mu3 = -3; 43 | mu4 = -3; 44 | mu5 = -2+1i; 45 | mu6 = -2-1i; 46 | mu7 = -6; 47 | mu8 = -6; 48 | Amu1 = [Ah_Linear-mu1*eye(8) Bh_Linear]; 49 | Amu2 = [Ah_Linear-mu2*eye(8) Bh_Linear]; 50 | Amu3 = [Ah_Linear-mu3*eye(8) Bh_Linear]; 51 | Amu4 = [Ah_Linear-mu4*eye(8) Bh_Linear]; 52 | Amu5 = [Ah_Linear-mu5*eye(8) Bh_Linear]; 53 | Amu6 = [Ah_Linear-mu6*eye(8) Bh_Linear]; 54 | Amu7 = [Ah_Linear-mu7*eye(8) Bh_Linear]; 55 | Amu8 = [Ah_Linear-mu8*eye(8) Bh_Linear]; 56 | 57 | S1 = null(Amu1); 58 | S2 = null(Amu2); 59 | S3 = null(Amu3); 60 | S4 = null(Amu4); 61 | S5 = null(Amu5); 62 | S6 = null(Amu6); 63 | S7 = null(Amu7); 64 | S8 = null(Amu8); 65 | 66 | v1q1 = S1(:,1); % n=8, m=2, v=n*1, q=m*1 67 | v2q2 = S2(:,2); 68 | v3q3 = S3(:,1); 69 | v4q4 = S4(:,2); 70 | v5q5 = real(S5(:,1)); 71 | v6q6 = imag(S5(:,1)); 72 | v7q7 = S7(:,1); 73 | v8q8 = S8(:,2); 74 | 75 | v1 = v1q1(1:8); q1 = v2q2(9:10); 76 | v2 = v2q2(1:8); q2 = v2q2(9:10); 77 | v3 = v3q3(1:8); q3 = v3q3(9:10); 78 | v4 = v4q4(1:8); q4 = v4q4(9:10); 79 | v5 = v5q5(1:8); q5 = v5q5(9:10); 80 | v6 = v6q6(1:8); q6 = v6q6(9:10); 81 | v7 = v7q7(1:8); q7 = v7q7(9:10); 82 | v8 = v8q8(1:8); q8 = v8q8(9:10); 83 | 84 | v = [v1 v2 v3 v4 v5 v6 v7 v8]; 85 | r_v = rank(v); 86 | 87 | if r_v == min(size(v)) 88 | 89 | fprintf('The matrix v is invertable and its rank is\n') 90 | disp(r_v) 91 | 92 | vi = inv(v); 93 | K = -[q1 q2 q3 q4 q5 q6 q7 q8]*vi; 94 | fprintf('The Controller Gain "K" is\n') 95 | disp(K) 96 | 97 | else 98 | 99 | disp('The v vectors must be selected independable') 100 | 101 | end 102 | 103 | %% Simulation 104 | T = 60; 105 | dt = 0.001; 106 | X0 = [0;0.2;0.0872665;-0.0523599;0.174533;-0.0872665;0;0]; 107 | t = 0; 108 | X(:,1) = X0; 109 | Time(1) = t; 110 | k = 1; 111 | while t < T 112 | Xj=X(:,k); 113 | yr1(k)=1*sign(sin(0.25*t)); 114 | yr2(k)=(0.0523599)*sign(sin(0.5*t)); 115 | u=-K*Xj; 116 | D1=MIMO_Double_Pendulum_Servo_Proj(t,Xj,u,yr1(k),yr2(k)); 117 | D2=MIMO_Double_Pendulum_Servo_Proj(t+dt/2,Xj+D1*dt/2,u,yr1(k),yr2(k)); 118 | D3=MIMO_Double_Pendulum_Servo_Proj(t+dt/2,Xj+D2*dt/2,u,yr1(k),yr2(k)); 119 | D4=MIMO_Double_Pendulum_Servo_Proj(t+dt,Xj+D3*dt,u,yr1(k),yr2(k)); 120 | Xj=Xj+(D1+2*D2+2*D3+D4)/6*dt; 121 | X(:,k+1)=Xj; 122 | Time(k+1)=t+dt; 123 | k=k+1; 124 | t=t+dt; 125 | end 126 | 127 | %% Plots 128 | figure; 129 | subplot(6,1,1);plot(Time,X(1,:),Time(1:end-1),yr1,'g'); 130 | title('Servo Design ') 131 | xlabel('time(s)') 132 | ylabel('X(m)') 133 | legend('Y1','Yr1','location','northeast') 134 | 135 | subplot(6,1,2);plot(Time,X(2,:)); 136 | xlabel('time(s)') 137 | ylabel('Xdot(m/s)') 138 | 139 | subplot(6,1,3);plot(Time,X(3,:),Time(1:end-1),yr2,'g'); 140 | xlabel('time(s)') 141 | ylabel('theta1(rad)') 142 | legend('Y2','Yr2','location','northeast') 143 | 144 | subplot(6,1,4);plot(Time,X(4,:)); 145 | xlabel('time(s)') 146 | ylabel('theta1dot(rad/s)') 147 | 148 | subplot(6,1,5);plot(Time,X(5,:)); 149 | xlabel('time(s)') 150 | ylabel('theta2(rad)') 151 | 152 | subplot(6,1,6);plot(Time,X(6,:)); 153 | xlabel('time(s)') 154 | ylabel('theta2dot(rad/s)') -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer/MIMO_Double_Pendulum_Obser_Servo_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=MIMO_Double_Pendulum_Obser_Servo_Proj(t,Xh,u,y) 2 | global C Ko 3 | 4 | DXh1 = Xh(2); 5 | DXh2 = (140*(cos(Xh(5)) - cos(Xh(3))*cos(Xh(3) - Xh(5)))*((sin(Xh(3) - Xh(5))*Xh(4)^2)/20 + sin(Xh(5))))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (20*(7*cos(Xh(3)) - 2*cos(Xh(5))*cos(Xh(3) - Xh(5)))*(u(2) + (7*sin(Xh(3)))/2 - (Xh(6)^2*sin(Xh(3) - Xh(5)))/20))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (10*(2*cos(Xh(3) - Xh(5))^2 - 7)*((7*sin(Xh(3))*Xh(4)^2)/20 + (sin(Xh(5))*Xh(6)^2)/10 + u(1)))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119); 6 | DXh3 = Xh(4); 7 | DXh4 = (40*((sin(Xh(3) - Xh(5))*Xh(4)^2)/20 + sin(Xh(5)))*(17*cos(Xh(3) - Xh(5)) - 7*cos(Xh(3))*cos(Xh(5))))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (40*(2*cos(Xh(5))^2 - 17)*(u(2) + (7*sin(Xh(3)))/2 - (Xh(6)^2*sin(Xh(3) - Xh(5)))/20))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (20*(7*cos(Xh(3)) - 2*cos(Xh(5))*cos(Xh(3) - Xh(5)))*((7*sin(Xh(3))*Xh(4)^2)/20 + (sin(Xh(5))*Xh(6)^2)/10 + u(1)))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119); 8 | DXh5 = Xh(6); 9 | DXh6 = (40*(17*cos(Xh(3) - Xh(5)) - 7*cos(Xh(3))*cos(Xh(5)))*(u(2) + (7*sin(Xh(3)))/2 - (Xh(6)^2*sin(Xh(3) - Xh(5)))/20))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (140*(cos(Xh(5)) - cos(Xh(3))*cos(Xh(3) - Xh(5)))*((7*sin(Xh(3))*Xh(4)^2)/20 + (sin(Xh(5))*Xh(6)^2)/10 + u(1)))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119) + (140*((sin(Xh(3) - Xh(5))*Xh(4)^2)/20 + sin(Xh(5)))*(7*cos(Xh(3))^2 - 17))/(49*cos(Xh(3))^2 + 14*cos(Xh(5))^2 + 34*cos(Xh(3) - Xh(5))^2 - 28*cos(Xh(3))*cos(Xh(5))*cos(Xh(3) - Xh(5)) - 119); 10 | yh = C*Xh; 11 | DXh = [DXh1;DXh2;DXh3;DXh4;DXh5;DXh6] + Ko*(y-yh); -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer/MIMO_Double_Pendulum_Servo_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_Double_Pendulum_Servo_Obs_Proj(t,X,u,yr1,yr2) 2 | global C 3 | 4 | Xx=X(1:6); 5 | Xi1=X(7); 6 | Xi2=X(8); 7 | y=C*Xx; 8 | 9 | DX1 = X(2); 10 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 11 | DX3 = X(4); 12 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 13 | DX5 = X(6); 14 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 15 | DXx= [DX1;DX2;DX3;DX4;DX5;DX6]; 16 | 17 | DXi1=yr1-y(1); 18 | DXi2=yr2-y(2); 19 | DX=[DXx;DXi1;DXi2]; -------------------------------------------------------------------------------- /Continous_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer/MIMO_Inverted_Double_Pendulum_System_Servo_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Servo_Observer_Design 2 | clc 3 | clear 4 | global C Ko 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | Ah_Linear = [A_Linear zeros(6,2);-C zeros(2,2)]; %n=8, m=2 11 | Bh_Linear = [B_Linear;zeros(2,2)]; 12 | %% Controllability and Observability 13 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear Ah_Linear^5*Bh_Linear Ah_Linear^6*Bh_Linear]; 14 | r_Mh = rank(Mh); 15 | if r_Mh == min(size(Mh)) 16 | 17 | fprintf('The system is controllable and the rank of Mh is\n') 18 | disp(r_Mh) 19 | 20 | else 21 | 22 | disp('The System is Uncontrollable') 23 | 24 | end 25 | 26 | N = [C;C*A_Linear;C*A_Linear^2;C*A_Linear^3;C*A_Linear^4;C*A_Linear^5]; 27 | r_N = rank(N); 28 | if r_N == min(size(N)) 29 | 30 | fprintf('The system is observable and the rank of N is\n') 31 | disp(r_N) 32 | 33 | else 34 | 35 | disp('The System is Unobservable') 36 | 37 | end 38 | 39 | %% Designing Controller Gain, EESA Method 40 | mu1 = -4; 41 | mu2 = -4; 42 | mu3 = -3; 43 | mu4 = -3; 44 | mu5 = -2+1i; 45 | mu6 = -2-1i; 46 | mu7 = -6; 47 | mu8 = -6; 48 | Amu1 = [Ah_Linear-mu1*eye(8) Bh_Linear]; 49 | Amu2 = [Ah_Linear-mu2*eye(8) Bh_Linear]; 50 | Amu3 = [Ah_Linear-mu3*eye(8) Bh_Linear]; 51 | Amu4 = [Ah_Linear-mu4*eye(8) Bh_Linear]; 52 | Amu5 = [Ah_Linear-mu5*eye(8) Bh_Linear]; 53 | Amu6 = [Ah_Linear-mu6*eye(8) Bh_Linear]; 54 | Amu7 = [Ah_Linear-mu7*eye(8) Bh_Linear]; 55 | Amu8 = [Ah_Linear-mu8*eye(8) Bh_Linear]; 56 | 57 | S1 = null(Amu1); 58 | S2 = null(Amu2); 59 | S3 = null(Amu3); 60 | S4 = null(Amu4); 61 | S5 = null(Amu5); 62 | S6 = null(Amu6); 63 | S7 = null(Amu7); 64 | S8 = null(Amu8); 65 | 66 | v1q1 = S1(:,1); % n=8, m=2, v=n*1, q=m*1 67 | v2q2 = S2(:,2); 68 | v3q3 = S3(:,1); 69 | v4q4 = S4(:,2); 70 | v5q5 = real(S5(:,1)); 71 | v6q6 = imag(S5(:,1)); 72 | v7q7 = S7(:,1); 73 | v8q8 = S8(:,2); 74 | 75 | v1 = v1q1(1:8); q1 = v2q2(9:10); 76 | v2 = v2q2(1:8); q2 = v2q2(9:10); 77 | v3 = v3q3(1:8); q3 = v3q3(9:10); 78 | v4 = v4q4(1:8); q4 = v4q4(9:10); 79 | v5 = v5q5(1:8); q5 = v5q5(9:10); 80 | v6 = v6q6(1:8); q6 = v6q6(9:10); 81 | v7 = v7q7(1:8); q7 = v7q7(9:10); 82 | v8 = v8q8(1:8); q8 = v8q8(9:10); 83 | 84 | v = [v1 v2 v3 v4 v5 v6 v7 v8]; 85 | r_v = rank(v); 86 | 87 | if r_v == min(size(v)) 88 | 89 | fprintf('The matrix v is invertable and its rank is\n') 90 | disp(r_v) 91 | 92 | vi = inv(v); 93 | K = -[q1 q2 q3 q4 q5 q6 q7 q8]*vi; 94 | fprintf('The Controller Gain "K" is\n') 95 | disp(K) 96 | 97 | else 98 | 99 | disp('The v vectors must be selected independable') 100 | 101 | end 102 | 103 | %% Designing Observer Gain, EESA Method 104 | NT = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C' (A_Linear')^4*C' (A_Linear')^5*C']; 105 | r_NT = rank(NT); 106 | if r_NT == min(size(NT)) 107 | 108 | fprintf('The system is observable and the rank of NT is\n') 109 | disp(r_NT) 110 | 111 | else 112 | 113 | disp('The System is Unobservable') 114 | 115 | end 116 | 117 | muo1 = -5; 118 | muo2 = -6; 119 | muo3 = -7; 120 | muo4 = -8; 121 | muo5 = -9; 122 | muo6 = -10; 123 | Amuo1 = [A_Linear'-muo1*eye(6) C']; 124 | Amuo2 = [A_Linear'-muo2*eye(6) C']; 125 | Amuo3 = [A_Linear'-muo3*eye(6) C']; 126 | Amuo4 = [A_Linear'-muo4*eye(6) C']; 127 | Amuo5 = [A_Linear'-muo5*eye(6) C']; 128 | Amuo6 = [A_Linear'-muo6*eye(6) C']; 129 | 130 | S1o = null(Amuo1); 131 | S2o = null(Amuo2); 132 | S3o = null(Amuo3); 133 | S4o = null(Amuo4); 134 | S5o = null(Amuo5); 135 | S6o = null(Amuo6); 136 | 137 | v1q1o = S1o(:,2); % n=6, m=2, v=n*1, q=m*1 138 | v2q2o = S2o(:,1); 139 | v3q3o = S3o(:,1); 140 | v4q4o = S4o(:,1); 141 | v5q5o = S5o(:,2); 142 | v6q6o = S6o(:,2)-S6o(:,1); 143 | v1o = v1q1o(1:6); q1o = v2q2o(7:8); 144 | v2o = v2q2o(1:6); q2o = v2q2o(7:8); 145 | v3o = v3q3o(1:6); q3o = v3q3o(7:8); 146 | v4o = v4q4o(1:6); q4o = v4q4o(7:8); 147 | v5o = v5q5o(1:6); q5o = v5q5o(7:8); 148 | v6o = v6q6o(1:6); q6o = v6q6o(7:8); 149 | 150 | vo = [v1o v2o v3o v4o v5o v6o]; 151 | r_vo = rank(vo); 152 | 153 | if r_vo == min(size(vo)) 154 | 155 | fprintf('The matrix vo is invertable and the its rank is\n') 156 | disp(r_vo) 157 | 158 | vio = inv(vo); 159 | KoT = -[q1o q2o q3o q4o q5o q6o]*vio; 160 | Ko = KoT'; 161 | fprintf('The Observer Gain "Ko" is\n') 162 | disp(Ko) 163 | 164 | else 165 | 166 | disp('The vo vectors must be selected independable') 167 | 168 | end 169 | %% Simulation 170 | T = 60; 171 | dt = 0.001; 172 | X0 = [0;0.2/2;0.0872665/2;-0.0523599/2;0.174533/2;-0.0872665/2;0;0]; 173 | Xh0 = [0.01;0.07;0.03/2;-0.08/2;1.5/2;-0.3/2]; 174 | t = 0; 175 | X(:,1) = X0; 176 | Xh(:,1) = Xh0; 177 | Time(1) = t; 178 | k = 1; 179 | while t < T 180 | Xj = X(:,k); 181 | Xhj = Xh(:,k); 182 | y = C*Xj(1:6); 183 | yr1(k)=1*sign(sin(0.25*t)); 184 | yr2(k)=(0.0523599)*sign(sin(0.5*t)); 185 | u = -[K(1,1:6);K(2,1:6)]*Xhj-[K(1,7:8);K(2,7:8)]*Xj(7:8); 186 | D1 = MIMO_Double_Pendulum_Servo_Obs_Proj(t,Xj,u,yr1(k),yr2(k)); 187 | D2 = MIMO_Double_Pendulum_Servo_Obs_Proj(t+dt/2,Xj+D1*dt/2,u,yr1(k),yr2(k)); 188 | D3 = MIMO_Double_Pendulum_Servo_Obs_Proj(t+dt/2,Xj+D2*dt/2,u,yr1(k),yr2(k)); 189 | D4 = MIMO_Double_Pendulum_Servo_Obs_Proj(t+dt,Xj+D3*dt,u,yr1(k),yr2(k)); 190 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 191 | X(:,k+1) = Xj; 192 | O1 = MIMO_Double_Pendulum_Obser_Servo_Proj(t,Xhj,u,y); 193 | O2 = MIMO_Double_Pendulum_Obser_Servo_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 194 | O3 = MIMO_Double_Pendulum_Obser_Servo_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 195 | O4 = MIMO_Double_Pendulum_Obser_Servo_Proj(t+dt,Xhj+O3*dt,u,y); 196 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 197 | Xh(:,k+1) = Xhj; 198 | Time(k+1) = t + dt; 199 | k = k + 1; 200 | t = t + dt; 201 | end 202 | 203 | %% Plots 204 | figure; 205 | subplot(6,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g',Time(1:end-1),yr1,'r'); 206 | title('Servo With Observer Design ') 207 | xlabel('time(s)') 208 | ylabel('X(m)') 209 | legend('X','Xhat','Yr1','location','northeast') 210 | 211 | subplot(6,1,2);plot(Time,X(2,:),Time,Xh(2,:),'g'); 212 | xlabel('time(s)') 213 | ylabel('Xdot(m/s)') 214 | legend('X','Xhat','location','northeast') 215 | 216 | subplot(6,1,3);plot(Time,X(3,:),Time,Xh(3,:),'g',Time(1:end-1),yr2,'r'); 217 | xlabel('time(s)') 218 | ylabel('theta1(rad)') 219 | legend('X','Xhat','Yr2','location','northeast') 220 | 221 | subplot(6,1,4);plot(Time,X(4,:),Time,Xh(4,:),'g'); 222 | xlabel('time(s)') 223 | ylabel('theta1dot(rad/s)') 224 | legend('X','Xhat','location','northeast') 225 | 226 | subplot(6,1,5);plot(Time,X(5,:),Time,Xh(5,:),'g'); 227 | xlabel('time(s)') 228 | ylabel('theta2(rad)') 229 | legend('X','Xhat','location','northeast') 230 | 231 | subplot(6,1,6);plot(Time,X(6,:),Time,Xh(6,:),'g'); 232 | xlabel('time(s)') 233 | ylabel('theta2dot(rad/s)') 234 | legend('X','Xhat','location','northeast') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Regulator_Design/MIMO_DOuble_Pendulum_Regul_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_DOuble_Pendulum_Regul_Proj(t,X,u) 2 | 3 | DX1 = X(2); 4 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 5 | DX3 = X(4); 6 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 7 | DX5 = X(6); 8 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 9 | DX= [DX1;DX2;DX3;DX4;DX5;DX6]; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Regulator_Design/MIMO_Inverted_Double_Pendulum_System_Regulator_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Regulator_Design 2 | clc 3 | clear 4 | 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | 11 | h = 0.05; 12 | G = expm(A_Linear*h); 13 | H = (eye(6) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 14 | 15 | mu_dcon = [-3.01 -2.99 -1.01 -1.99 -2+1i -2-1i]; 16 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h) exp(mu_dcon(5))*h exp(mu_dcon(6))*h]; 17 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 18 | disp(mu_ddis) 19 | 20 | muo_dcon = [-10 -10.01 -10.02 -10.03 -9.99 -9.98]; 21 | muo_ddis = [exp(muo_dcon(1)*h) exp(muo_dcon(2)*h) exp(muo_dcon(3)*h) exp(muo_dcon(4)*h) exp(muo_dcon(5)*h) exp(muo_dcon(6)*h)]; 22 | fprintf('The closed loop discerete time approximated system desired eigen values must be placed at\n') 23 | disp(muo_ddis) 24 | 25 | %% Designing Controller Gain, Place Method 26 | M_Continous = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear A_Linear^4*B_Linear]; 27 | r_M_Continous = rank(M_Continous); 28 | M_Discerete = [H G*H G^2*H G^3*H G^4*H]; 29 | r_M_Discerete = rank(M_Discerete); 30 | 31 | if r_M_Continous == min(size(M_Continous)) 32 | 33 | fprintf('The contionous time system is controllable and the rank of M is\n') 34 | disp(r_M_Continous) 35 | 36 | 37 | else 38 | disp('The continous time system is unctrollable') 39 | end 40 | 41 | if r_M_Discerete == min(size(M_Discerete)) 42 | 43 | fprintf('The discerete time system is controllable and the rank of M is\n') 44 | disp(r_M_Discerete) 45 | 46 | K = place(G,H,mu_ddis); 47 | 48 | fprintf('The Controller Gain "K" is\n') 49 | disp(K) 50 | 51 | else 52 | disp('The continous time system is unctrollable') 53 | end 54 | 55 | %% Designing Observer Gain, Place Method 56 | N_T_Contionous = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C' (A_Linear')^4*C']; 57 | r_N_Contionous = rank(N_T_Contionous); 58 | N_T_Discerete = [C' G'*C' (G')^2*C' (G')^3*C' (G')^4*C']; 59 | r_N_Discerete = rank(N_T_Discerete); 60 | 61 | if r_N_Contionous == min(size(N_T_Contionous)) 62 | 63 | fprintf('The contiouns time system is Observable and the rank of N_Transpose is\n') 64 | disp(r_N_Contionous) 65 | 66 | else 67 | disp('The continous time system is unobservable') 68 | end 69 | 70 | if r_N_Discerete == min(size(N_T_Discerete)) 71 | 72 | fprintf('The discerete time system is Observable and the rank of N_Transpose is\n') 73 | disp(r_N_Discerete) 74 | 75 | Ko = place(G',C',muo_ddis); 76 | Ko = Ko'; 77 | 78 | fprintf('The observer gain "Ko" is\n') 79 | disp(Ko) 80 | 81 | else 82 | disp('The discerete time system is unobservable') 83 | end 84 | 85 | %% Simulation 86 | T = 10; 87 | dt = 0.001; 88 | X0 = [0;0.2;0.0872665;-0.0523599;0.174533;-0.0872665]; 89 | t = 0; 90 | Time(1) = t; 91 | k = 0; 92 | i = 1; 93 | X(:,i) = X0; 94 | while t < T 95 | Xj = X(:,i); 96 | 97 | if mod(i,floor(h/dt))==1 98 | k=k+1; 99 | ud = -K*Xj; 100 | ud(:,k) = ud; 101 | end 102 | 103 | uj = ud(:,k); 104 | u(:,i) = uj; 105 | D1 = MIMO_DOuble_Pendulum_Regul_Proj(t,Xj,uj); 106 | D2 = MIMO_DOuble_Pendulum_Regul_Proj(t+dt/2,Xj+D1*dt/2,uj); 107 | D3 = MIMO_DOuble_Pendulum_Regul_Proj(t+dt/2,Xj+D2*dt/2,uj); 108 | D4 = MIMO_DOuble_Pendulum_Regul_Proj(t+dt,Xj+D3*dt,uj); 109 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 110 | X(:,i+1) = Xj; 111 | Time(i+1) = t + dt; 112 | i = i+1; 113 | t = t + dt; 114 | end 115 | 116 | %% Plots 117 | figure; 118 | subplot(6,1,1);plot(Time,X(1,:)); 119 | title('Regulator Controller Design and Sampling Time "0.05"') 120 | xlabel('time(s)') 121 | ylabel('x(m)') 122 | 123 | subplot(6,1,2);plot(Time,X(2,:)); 124 | xlabel('time(s)') 125 | ylabel('xdot(m/s)') 126 | 127 | subplot(6,1,3);plot(Time,X(3,:)); 128 | xlabel('time(s)') 129 | ylabel('theta1(rad)') 130 | 131 | subplot(6,1,4);plot(Time,X(4,:)); 132 | xlabel('time(s)') 133 | ylabel('theta1dot(rad/s)') 134 | 135 | subplot(6,1,5);plot(Time,X(5,:)); 136 | xlabel('time(s)') 137 | ylabel('theta2(rad)') 138 | 139 | subplot(6,1,6);plot(Time,X(6,:)); 140 | xlabel('time(s)') 141 | ylabel('theta2dot(rad/s)') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design/MIMO_Double_Pendulum_Regul_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_Double_Pendulum_Regul_Obs_Proj(t,X,u) 2 | 3 | DX1 = X(2); 4 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 5 | DX3 = X(4); 6 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 7 | DX5 = X(6); 8 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 9 | DX= [DX1;DX2;DX3;DX4;DX5;DX6]; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Regulator_Observer_Design/MIMO_Inverted_Double_Pendulum_System_Regulator_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Regulator_Design 2 | clc 3 | clear 4 | 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | 11 | h = 0.05; 12 | G = expm(A_Linear*h); 13 | H = (eye(6) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 14 | 15 | mu_dcon = [-3.01 -2.99 -1.01 -1.99 -2+1i -2-1i]; 16 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h) exp(mu_dcon(5))*h exp(mu_dcon(6))*h]; 17 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 18 | disp(mu_ddis) 19 | 20 | muo_dcon = [-10 -10.01 -10.02 -10.03 -9.99 -9.98]; 21 | muo_ddis = [exp(muo_dcon(1)*h) exp(muo_dcon(2)*h) exp(muo_dcon(3)*h) exp(muo_dcon(4)*h) exp(muo_dcon(5)*h) exp(muo_dcon(6)*h)]; 22 | fprintf('The closed loop discerete time approximated system desired eigen values must be placed at\n') 23 | disp(muo_ddis) 24 | 25 | %% Designing Controller Gain, Place Method 26 | M_Continous = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear A_Linear^4*B_Linear]; 27 | r_M_Continous = rank(M_Continous); 28 | M_Discerete = [H G*H G^2*H G^3*H G^4*H]; 29 | r_M_Discerete = rank(M_Discerete); 30 | 31 | if r_M_Continous == min(size(M_Continous)) 32 | 33 | fprintf('The contionous time system is controllable and the rank of M is\n') 34 | disp(r_M_Continous) 35 | 36 | 37 | else 38 | disp('The continous time system is unctrollable') 39 | end 40 | 41 | if r_M_Discerete == min(size(M_Discerete)) 42 | 43 | fprintf('The discerete time system is controllable and the rank of M is\n') 44 | disp(r_M_Discerete) 45 | 46 | K = place(G,H,mu_ddis); 47 | 48 | fprintf('The Controller Gain "K" is\n') 49 | disp(K) 50 | 51 | else 52 | disp('The continous time system is unctrollable') 53 | end 54 | 55 | %% Designing Observer Gain, Place Method 56 | N_T_Contionous = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C' (A_Linear')^4*C']; 57 | r_N_Contionous = rank(N_T_Contionous); 58 | N_T_Discerete = [C' G'*C' (G')^2*C' (G')^3*C' (G')^4*C']; 59 | r_N_Discerete = rank(N_T_Discerete); 60 | 61 | if r_N_Contionous == min(size(N_T_Contionous)) 62 | 63 | fprintf('The contiouns time system is Observable and the rank of N_Transpose is\n') 64 | disp(r_N_Contionous) 65 | 66 | else 67 | disp('The continous time system is unobservable') 68 | end 69 | 70 | if r_N_Discerete == min(size(N_T_Discerete)) 71 | 72 | fprintf('The discerete time system is Observable and the rank of N_Transpose is\n') 73 | disp(r_N_Discerete) 74 | 75 | Ko = place(G',C',muo_ddis); 76 | Ko = Ko'; 77 | 78 | fprintf('The observer gain "Ko" is\n') 79 | disp(Ko) 80 | 81 | else 82 | disp('The discerete time system is unobservable') 83 | end 84 | 85 | %% Simulation 86 | T = 10; 87 | dt = 0.001; 88 | X0 = [0;0.02/2;0.008/2;-0.005/2;0.017/2;-0.008/2]; 89 | Xh0 = [0.0001;0.0001;0.0001;0.0001;0.0001;0.0001]; 90 | t = 0; 91 | Time(1) = t; 92 | k = 0; 93 | i = 1; 94 | X(:,i) = X0; 95 | Xh(:,1) = Xh0; 96 | 97 | while t < T 98 | 99 | Xj = X(:,i); 100 | y = C*Xj; 101 | 102 | if mod(i,floor(h/dt))==1 103 | k=k+1; 104 | ud = -K*Xh(:,k); 105 | ud(:,k) = ud; 106 | yh = C*Xh(:,k); 107 | Xh(:,k+1) = G*Xh(:,k) + H*ud(:,k) + Ko*(y-yh); 108 | 109 | end 110 | 111 | uj = ud(:,k); 112 | u(:,i) = uj; 113 | Xhj = Xh(:,k); 114 | Xh(:,i) = Xhj; 115 | 116 | D1 = MIMO_Double_Pendulum_Regul_Obs_Proj(t,Xj,uj); 117 | D2 = MIMO_Double_Pendulum_Regul_Obs_Proj(t+dt/2,Xj+D1*dt/2,uj); 118 | D3 = MIMO_Double_Pendulum_Regul_Obs_Proj(t+dt/2,Xj+D2*dt/2,uj); 119 | D4 = MIMO_Double_Pendulum_Regul_Obs_Proj(t+dt,Xj+D3*dt,uj); 120 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 121 | X(:,i+1) = Xj; 122 | Time(i+1) = t + dt; 123 | i = i + 1; 124 | t = t + dt; 125 | end 126 | 127 | %% Plots 128 | figure; 129 | subplot(6,1,1);plot(Time,X(1,:),Time(1:end-1),Xh(1,:),'g'); 130 | title('Regulator Controller With Observer Design and Samling time "0.05"') 131 | xlabel('time(s)') 132 | ylabel('x(m)') 133 | 134 | subplot(6,1,2);plot(Time,X(2,:),Time(1:end-1),Xh(2,:),'g'); 135 | xlabel('time(s)') 136 | ylabel('xdot(m/s)') 137 | 138 | subplot(6,1,3);plot(Time,X(3,:),Time(1:end-1),Xh(3,:),'g'); 139 | xlabel('time(s)') 140 | ylabel('theta1(rad)') 141 | 142 | subplot(6,1,4);plot(Time,X(4,:),Time(1:end-1),Xh(4,:),'g'); 143 | xlabel('time(s)') 144 | ylabel('theta1dot(rad/s)') 145 | 146 | subplot(6,1,5);plot(Time,X(5,:),Time(1:end-1),Xh(5,:),'g'); 147 | xlabel('time(s)') 148 | ylabel('theta2(rad)') 149 | 150 | subplot(6,1,6);plot(Time,X(6,:),Time(1:end-1),Xh(6,:),'g'); 151 | xlabel('time(s)') 152 | ylabel('theta2dot(rad/s)') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator/MIMO_Double_Pendulum_Servo_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_Double_Pendulum_Servo_Proj(t,X,u,yr1,yr2) 2 | global C 3 | 4 | Xx=X(1:6); 5 | Xi1=X(7); 6 | Xi2=X(8); 7 | y=C*Xx; 8 | 9 | DX1 = X(2); 10 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 11 | DX3 = X(4); 12 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 13 | DX5 = X(6); 14 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 15 | DXx= [DX1;DX2;DX3;DX4;DX5;DX6]; 16 | 17 | DXi1=yr1-y(1); 18 | DXi2=yr2-y(2); 19 | DX=[DXx;DXi1;DXi2]; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator/MIMO_Inverted_Double_Pendulum_System_Servo_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Servo_Design 2 | clc 3 | clear 4 | global C 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | h = 0.05; 11 | 12 | G = expm(A_Linear*h); 13 | H = (eye(6) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 14 | Gh = [G zeros(6,2);-C eye(2)]; 15 | Hh = [H;zeros(2,2)]; 16 | 17 | mu_dcon = [-4.01 -3.99 -3.01 -2.99 -2-2i -2+2i -6.01 -5.99]; 18 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h) exp(mu_dcon(5)*h) exp(mu_dcon(6)*h) exp(mu_dcon(7)*h) exp(mu_dcon(8)*h)]; 19 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 20 | disp(mu_ddis) 21 | 22 | 23 | %% Designing Controller Gain, Place Method 24 | Mh_Discerete = [Hh Gh*Hh Gh^2*Hh Gh^3*Hh Gh^4*Hh Gh^5*Hh Gh^6*Hh]; 25 | r_Mh_Discerete = rank(Mh_Discerete); 26 | 27 | if r_Mh_Discerete == min(size(Mh_Discerete)) 28 | 29 | fprintf('The discerete time system is controllable and the rank of Mh is\n') 30 | disp(r_Mh_Discerete) 31 | 32 | K = place(Gh,Hh,mu_ddis); 33 | 34 | fprintf('The Controller Gain "K" is\n') 35 | disp(K) 36 | 37 | else 38 | disp('The continous time system is unctrollable') 39 | end 40 | 41 | %% Simulation 42 | T = 60; 43 | dt = 0.001; 44 | X0 = [0;0.2;0.0872665;-0.0523599;0.174533;-0.0872665;0;0]; 45 | t = 0; 46 | Time(1) = t; 47 | k = 0; 48 | i = 1; 49 | X(:,1) = X0; 50 | 51 | while t < T 52 | 53 | Xj = X(:,i); 54 | 55 | if mod(i,floor(h/dt))==1 56 | 57 | k=k+1; 58 | ud = -K*Xj; 59 | ud(:,k) = ud; 60 | 61 | end 62 | 63 | 64 | yr1(i)=1*sign(sin(0.25*t)); 65 | yr2(i)=(0.0523599)*sign(sin(0.5*t)); 66 | 67 | uj = ud(:,k); 68 | u(:,i) = uj; 69 | 70 | D1=MIMO_Double_Pendulum_Servo_Proj(t,Xj,uj,yr1(i),yr2(i)); 71 | D2=MIMO_Double_Pendulum_Servo_Proj(t+dt/2,Xj+D1*dt/2,uj,yr1(i),yr2(i)); 72 | D3=MIMO_Double_Pendulum_Servo_Proj(t+dt/2,Xj+D2*dt/2,uj,yr1(i),yr2(i)); 73 | D4=MIMO_Double_Pendulum_Servo_Proj(t+dt,Xj+D3*dt,uj,yr1(i),yr2(i)); 74 | Xj=Xj+(D1+2*D2+2*D3+D4)/6*dt; 75 | X(:,i+1) = Xj; 76 | Time(i+1)= t + dt; 77 | i = i+1; 78 | t = t+dt; 79 | end 80 | 81 | %% Plots 82 | figure; 83 | subplot(6,1,1);plot(Time,X(1,:),Time(1:end-1),yr1,'g'); 84 | title('Servo Design Signal Frequencies "0.25 & 0.5" and Smapling Time "0.05"') 85 | xlabel('time(s)') 86 | ylabel('X(m)') 87 | legend('Y1','Yr1','location','northeast') 88 | 89 | subplot(6,1,2);plot(Time,X(2,:)); 90 | xlabel('time(s)') 91 | ylabel('Xdot(m/s)') 92 | 93 | subplot(6,1,3);plot(Time,X(3,:),Time(1:end-1),yr2,'g'); 94 | xlabel('time(s)') 95 | ylabel('theta1(rad)') 96 | legend('Y2','Yr2','location','northeast') 97 | 98 | subplot(6,1,4);plot(Time,X(4,:)); 99 | xlabel('time(s)') 100 | ylabel('theta1dot(rad/s)') 101 | 102 | subplot(6,1,5);plot(Time,X(5,:)); 103 | xlabel('time(s)') 104 | ylabel('theta2(rad)') 105 | 106 | subplot(6,1,6);plot(Time,X(6,:)); 107 | xlabel('time(s)') 108 | ylabel('theta2dot(rad/s)') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer/MIMO_Double_Pendulum_Servo_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=MIMO_Double_Pendulum_Servo_Proj(t,X,u,yr1,yr2) 2 | global C 3 | 4 | Xx=X(1:6); 5 | Xi1=X(7); 6 | Xi2=X(8); 7 | y=C*Xx; 8 | 9 | DX1 = X(2); 10 | DX2 = (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (10*(2*cos(X(3) - X(5))^2 - 7)*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 11 | DX3 = X(4); 12 | DX4 = (40*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5))))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (40*(2*cos(X(5))^2 - 17)*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (20*(7*cos(X(3)) - 2*cos(X(5))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 13 | DX5 = X(6); 14 | DX6 = (40*(17*cos(X(3) - X(5)) - 7*cos(X(3))*cos(X(5)))*(u(2) + (7*sin(X(3)))/2 - (X(6)^2*sin(X(3) - X(5)))/20))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*(cos(X(5)) - cos(X(3))*cos(X(3) - X(5)))*((7*sin(X(3))*X(4)^2)/20 + (sin(X(5))*X(6)^2)/10 + u(1)))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119) + (140*((sin(X(3) - X(5))*X(4)^2)/20 + sin(X(5)))*(7*cos(X(3))^2 - 17))/(49*cos(X(3))^2 + 14*cos(X(5))^2 + 34*cos(X(3) - X(5))^2 - 28*cos(X(3))*cos(X(5))*cos(X(3) - X(5)) - 119); 15 | DXx= [DX1;DX2;DX3;DX4;DX5;DX6]; 16 | 17 | DXi1=yr1-y(1); 18 | DXi2=yr2-y(2); 19 | DX=[DXx;DXi1;DXi2]; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Double_Pendulum_System_Servo_Adding_Integrator__Observer/MIMO_Inverted_Double_Pendulum_System_Servo_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_MIMO_Inverted_Double_Pendulum_System_Servo_Design 2 | clc 3 | clear 4 | global C 5 | %% System Paremeters 6 | % g = 10, M_cart = 1kg, m_1 = 0.5kg, m_2 = 0.2kg, l_1 = 0.5m, l_2 = 0.5m 7 | A_Linear = [0 1 0 0 0 0;0 0 -7 0 0 0;0 0 0 1 0 0;0 0 42 0 -8 0;0 0 0 0 0 1;0 0 -28 0 28 0]; 8 | B_Linear = [0 0;1 -2;0 0;-2 12;0 0;0 -8]; 9 | C = [1 0 0 0 0 0;0 0 1 0 0 0]; 10 | h = 0.05; 11 | 12 | G = expm(A_Linear*h); 13 | H = (eye(6) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 14 | Gh = [G zeros(6,2);-C eye(2)]; 15 | Hh = [H;zeros(2,2)]; 16 | 17 | mu_dcon = [-4.01 -3.99 -3.01 -2.99 -2-2i -2+2i -6.01 -5.99]; 18 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h) exp(mu_dcon(5)*h) exp(mu_dcon(6)*h) exp(mu_dcon(7)*h) exp(mu_dcon(8)*h)]; 19 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 20 | disp(mu_ddis) 21 | 22 | muo_dcon = [-10 -10.01 -10.02 -10.03 -9.99 -9.98]; 23 | muo_ddis = [exp(muo_dcon(1)*h) exp(muo_dcon(2)*h) exp(muo_dcon(3)*h) exp(muo_dcon(4)*h) exp(muo_dcon(5)*h) exp(muo_dcon(6)*h)]; 24 | fprintf('The closed loop discerete time approximated system desired eigen values must be placed at\n') 25 | disp(muo_ddis) 26 | 27 | %% Designing Controller Gain, Place Method 28 | Mh_Discerete = [Hh Gh*Hh Gh^2*Hh Gh^3*Hh Gh^4*Hh Gh^5*Hh Gh^6*Hh]; 29 | r_Mh_Discerete = rank(Mh_Discerete); 30 | 31 | if r_Mh_Discerete == min(size(Mh_Discerete)) 32 | 33 | fprintf('The discerete time system is controllable and the rank of Mh is\n') 34 | disp(r_Mh_Discerete) 35 | 36 | K = place(Gh,Hh,mu_ddis); 37 | 38 | fprintf('The Controller Gain "K" is\n') 39 | disp(K) 40 | 41 | else 42 | disp('The continous time system is unctrollable') 43 | end 44 | 45 | %% Designing Observer Gain, Place Method 46 | N_T_Contionous = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C' (A_Linear')^4*C']; 47 | r_N_Contionous = rank(N_T_Contionous); 48 | N_T_Discerete = [C' G'*C' (G')^2*C' (G')^3*C' (G')^4*C']; 49 | r_N_Discerete = rank(N_T_Discerete); 50 | 51 | if r_N_Contionous == min(size(N_T_Contionous)) 52 | 53 | fprintf('The contiouns time system is Observable and the rank of N_Transpose is\n') 54 | disp(r_N_Contionous) 55 | 56 | else 57 | disp('The continous time system is unobservable') 58 | end 59 | 60 | if r_N_Discerete == min(size(N_T_Discerete)) 61 | 62 | fprintf('The discerete time system is Observable and the rank of N_Transpose is\n') 63 | disp(r_N_Discerete) 64 | 65 | Ko = place(G',C',muo_ddis); 66 | Ko = Ko'; 67 | 68 | fprintf('The observer gain "Ko" is\n') 69 | disp(Ko) 70 | 71 | else 72 | disp('The discerete time system is unobservable') 73 | end 74 | 75 | %% Simulation 76 | T = 400; 77 | dt = 0.001; 78 | X0 = [0;0.002;0.000872665;-0.000523599;0.00174533;-0.000872665;0;0]; 79 | Xh0 = [0.00001;0.00001;0.00001;0.00001;0.00001;0.00001]; 80 | t = 0; 81 | Time(1) = t; 82 | k = 0; 83 | i = 1; 84 | X(:,1) = X0; 85 | Xh(:,1) = Xh0; 86 | 87 | while t < T 88 | 89 | Xj = X(:,i); 90 | y = C*Xj(1:6); 91 | 92 | if mod(i,floor(h/dt))==1 93 | 94 | k=k+1; 95 | ud = -K(1:2,1:6)*Xh(:,k) - K(1:2,7:8)*Xj(7:8); 96 | ud(:,k) = ud; 97 | yh = C*Xh(:,k); 98 | Xh(:,k+1) = G*Xh(:,k) + H*ud(:,k) + Ko*(y-yh); 99 | 100 | end 101 | 102 | 103 | yr1(i)=1*sign(sin(0.025*t)); 104 | yr2(i)=(0.0523599)*sign(sin(0.05*t)); 105 | 106 | uj = ud(:,k); 107 | u(:,i) = uj; 108 | Xhj = Xh(:,k); 109 | Xh(:,i) = Xhj; 110 | 111 | D1=MIMO_Double_Pendulum_Servo_Proj(t,Xj,uj,yr1(i),yr2(i)); 112 | D2=MIMO_Double_Pendulum_Servo_Proj(t+dt/2,Xj+D1*dt/2,uj,yr1(i),yr2(i)); 113 | D3=MIMO_Double_Pendulum_Servo_Proj(t+dt/2,Xj+D2*dt/2,uj,yr1(i),yr2(i)); 114 | D4=MIMO_Double_Pendulum_Servo_Proj(t+dt,Xj+D3*dt,uj,yr1(i),yr2(i)); 115 | Xj=Xj+(D1+2*D2+2*D3+D4)/6*dt; 116 | X(:,i+1) = Xj; 117 | Time(i+1)= t + dt; 118 | i = i+1; 119 | t = t+dt; 120 | end 121 | 122 | %% Plots 123 | figure; 124 | subplot(6,1,1);plot(Time,X(1,:),Time(1:end-1),Xh(1,:),'g',Time(1:end-1),yr1,'r'); 125 | title('Servo Design Signal Frequencies "0.025 & 0.05" and Smapling Time "0.05"') 126 | xlabel('time(s)') 127 | ylabel('X(m)') 128 | legend('Y1','Yr1','location','northeast') 129 | 130 | subplot(6,1,2);plot(Time,X(2,:),Time(1:end-1),Xh(2,:),'g'); 131 | xlabel('time(s)') 132 | ylabel('Xdot(m/s)') 133 | 134 | subplot(6,1,3);plot(Time,X(3,:),Time(1:end-1),Xh(3,:),'g',Time(1:end-1),yr2,'r'); 135 | xlabel('time(s)') 136 | ylabel('theta1(rad)') 137 | legend('Y2','Yr2','location','northeast') 138 | 139 | subplot(6,1,4);plot(Time,X(4,:),Time(1:end-1),Xh(4,:),'g'); 140 | xlabel('time(s)') 141 | ylabel('theta1dot(rad/s)') 142 | 143 | subplot(6,1,5);plot(Time,X(5,:),Time(1:end-1),Xh(5,:),'g'); 144 | xlabel('time(s)') 145 | ylabel('theta2(rad)') 146 | 147 | subplot(6,1,6);plot(Time,X(6,:),Time(1:end-1),Xh(6,:),'g'); 148 | xlabel('time(s)') 149 | ylabel('theta2dot(rad/s)') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Regulator_Design/Pendulum_Regul_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Regul_Proj(t,X,u) 2 | 3 | global M_Cart m g l A B 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Regulator_Design/Project_Inverted_Pendulum_System_Regulator_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_Inverted_Pendulum_System_Regulator_Design 2 | clc 3 | clear 4 | 5 | global M_Cart m g l 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | h = 0.05; 12 | 13 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 14 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 15 | C = [1 0 0 0]; 16 | G = expm(A_Linear*h); 17 | H = (eye(4) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 18 | 19 | mu_dcon = [-3 -3 -2+2i -2-2i]; 20 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h)]; 21 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 22 | disp(mu_ddis) 23 | 24 | muo_dcon = [-10 -10 -10 -10]; 25 | muo_ddis = [exp(muo_dcon(1)*h) exp(muo_dcon(2)*h) exp(muo_dcon(3)*h) exp(muo_dcon(4)*h)]; 26 | fprintf('The closed loop discerete time approximated system desired eigen values must be placed at\n') 27 | disp(muo_ddis) 28 | 29 | %% Designing Controller Gain, Ackerman Method 30 | M_Continous = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 31 | r_M_Continous = rank(M_Continous); 32 | M_Discerete = [H G*H G^2*H G^3*H]; 33 | r_M_Discerete = rank(M_Discerete); 34 | 35 | if r_M_Continous == min(size(M_Continous)) 36 | 37 | fprintf('The contionous time system is controllable and the rank of M is\n') 38 | disp(r_M_Continous) 39 | 40 | 41 | else 42 | disp('The continous time system is unctrollable') 43 | end 44 | 45 | if r_M_Discerete == min(size(M_Discerete)) 46 | 47 | fprintf('The discerete time system is controllable and the rank of M is\n') 48 | disp(r_M_Discerete) 49 | 50 | K = acker(G,H,mu_ddis); 51 | 52 | fprintf('The Controller Gain "K" is\n') 53 | disp(K) 54 | 55 | else 56 | disp('The continous time system is unctrollable') 57 | end 58 | 59 | %% Designing Observer Gain, Ackerman Method 60 | N_T_Contionous = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C']; 61 | r_N_Contionous = rank(N_T_Contionous); 62 | N_T_Discerete = [C' G'*C' (G')^2*C' (G')^3*C']; 63 | r_N_Discerete = rank(N_T_Discerete); 64 | 65 | if r_N_Contionous == min(size(N_T_Contionous)) 66 | 67 | fprintf('The contiouns time system is Observable and the rank of N_Transpose is\n') 68 | disp(r_N_Contionous) 69 | 70 | else 71 | disp('The continous time system is unobservable') 72 | end 73 | 74 | if r_N_Discerete == min(size(N_T_Discerete)) 75 | 76 | fprintf('The discerete time system is Observable and the rank of N_Transpose is\n') 77 | disp(r_N_Discerete) 78 | 79 | Ko = acker(G',C',muo_ddis); 80 | Ko = Ko'; 81 | 82 | fprintf('The observer gain "Ko" is\n') 83 | disp(Ko) 84 | 85 | else 86 | disp('The discerete time system is unobservable') 87 | end 88 | 89 | %% Simulation 90 | T = 5; 91 | dt = 0.001; 92 | X0 = [0;0;0.174533;0.0872665]; 93 | t = 0; 94 | Time(1) = t; 95 | k = 0; 96 | i = 1; 97 | X(:,i) = X0; 98 | while t < T 99 | Xj = X(:,i); 100 | 101 | if mod(i,floor(h/dt))==1 102 | k=k+1; 103 | ud(k)=-K*Xj; 104 | end 105 | 106 | u(:,i) = ud(k); 107 | D1 = Pendulum_Regul_Proj(t,Xj,u(i)); 108 | D2 = Pendulum_Regul_Proj(t+dt/2,Xj+D1*dt/2,u(i)); 109 | D3 = Pendulum_Regul_Proj(t+dt/2,Xj+D2*dt/2,u(i)); 110 | D4 = Pendulum_Regul_Proj(t+dt,Xj+D3*dt,u(i)); 111 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 112 | X(:,i+1) = Xj; 113 | Time(i+1) = t + dt; 114 | i = i+1; 115 | t = t + dt; 116 | end 117 | 118 | %% Plots 119 | figure; 120 | subplot(5,1,1);plot(Time,X(1,:)); 121 | title('Regulator Controller Design Initial Condition "0,0,10,5" and Smapling Time of "0.05"') 122 | xlabel('time(s)') 123 | ylabel('x(m)') 124 | 125 | subplot(5,1,2);plot(Time,X(2,:)); 126 | xlabel('time(s)') 127 | ylabel('xdot(m/s)') 128 | 129 | subplot(5,1,3);plot(Time,X(3,:)); 130 | xlabel('time(s)') 131 | ylabel('theta(rad)') 132 | 133 | subplot(5,1,4);plot(Time,X(4,:)); 134 | xlabel('time(s)') 135 | ylabel('thetadot(rad/s)') 136 | 137 | subplot(5,1,5);plot(Time(1:end-1),u); 138 | xlabel('time(s)') 139 | ylabel('ZOH Input') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Regulator_Observer_Design/Pendulum_Regul_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Regul_Obs_Proj(t,X,u) 2 | 3 | global A B M_Cart m g l 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Regulator_Observer_Design/Project_Inverted_Pendulum_System_Regulator_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_Inverted_Pendulum_System_Regulator_Observer_Design 2 | clc 3 | clear 4 | 5 | global M_Cart m g l Ko C 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | h = 0.01; 12 | 13 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 14 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 15 | C = [1 0 0 0]; 16 | G = expm(A_Linear*h); 17 | H = (eye(4) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 18 | 19 | mu_dcon = [-3 -3 -2+2i -2-2i]; 20 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h)]; 21 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 22 | disp(mu_ddis) 23 | 24 | muo_dcon = [-10 -10 -10 -10]; 25 | muo_ddis = [exp(muo_dcon(1)*h) exp(muo_dcon(2)*h) exp(muo_dcon(3)*h) exp(muo_dcon(4)*h)]; 26 | fprintf('The closed loop discerete time approximated system desired eigen values must be placed at\n') 27 | disp(muo_ddis) 28 | 29 | %% Designing Controller Gain, Ackerman Method 30 | M_Continous = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 31 | r_M_Continous = rank(M_Continous); 32 | M_Discerete = [H G*H G^2*H G^3*H]; 33 | r_M_Discerete = rank(M_Discerete); 34 | 35 | if r_M_Continous == min(size(M_Continous)) 36 | 37 | fprintf('The contionous time system is controllable and the rank of M is\n') 38 | disp(r_M_Continous) 39 | 40 | 41 | else 42 | disp('The continous time system is unctrollable') 43 | end 44 | 45 | if r_M_Discerete == min(size(M_Discerete)) 46 | 47 | fprintf('The discerete time system is controllable and the rank of M is\n') 48 | disp(r_M_Discerete) 49 | 50 | K = acker(G,H,mu_ddis); 51 | 52 | fprintf('The Controller Gain "K" is\n') 53 | disp(K) 54 | 55 | else 56 | disp('The continous time system is unctrollable') 57 | end 58 | 59 | %% Designing Observer Gain, Ackerman Method 60 | N_T_Contionous = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C']; 61 | r_N_Contionous = rank(N_T_Contionous); 62 | N_T_Discerete = [C' G'*C' (G')^2*C' (G')^3*C']; 63 | r_N_Discerete = rank(N_T_Discerete); 64 | 65 | if r_N_Contionous == min(size(N_T_Contionous)) 66 | 67 | fprintf('The contiouns time system is Observable and the rank of N_Transpose is\n') 68 | disp(r_N_Contionous) 69 | 70 | else 71 | disp('The continous time system is unobservable') 72 | end 73 | 74 | if r_N_Discerete == min(size(N_T_Discerete)) 75 | 76 | fprintf('The discerete time system is Observable and the rank of N_Transpose is\n') 77 | disp(r_N_Discerete) 78 | 79 | Ko = acker(G',C',muo_ddis); 80 | Ko = Ko'; 81 | 82 | fprintf('The observer gain "Ko" is\n') 83 | disp(Ko) 84 | 85 | else 86 | disp('The discerete time system is unobservable') 87 | end 88 | 89 | %% Simulation 90 | T = 10; 91 | dt = 0.001; 92 | X0 = [0;0;0.17;0.08]; 93 | Xh0 = [0.01;0.1;0.21;0.25]; 94 | t = 0; 95 | Time(1) = t; 96 | k = 0; 97 | i = 1; 98 | X(:,i) = X0; 99 | Xh(:,1) = Xh0; 100 | 101 | while t < T 102 | 103 | Xj = X(:,i); 104 | y = C*Xj; 105 | if mod(i,floor(h/dt))==1 106 | 107 | k=k+1; 108 | ud(k)=-K*Xh(:,k); 109 | yh = C*Xh(:,k); 110 | Xh(:,k+1) = G*Xh(:,k) + H*ud(k) + Ko*(y-yh); 111 | end 112 | 113 | u(:,i) = ud(k); 114 | Xhj = Xh(:,k); 115 | Xh(:,i) = Xhj; 116 | 117 | D1 = Pendulum_Regul_Obs_Proj(t,Xj,u(i)); 118 | D2 = Pendulum_Regul_Obs_Proj(t+dt/2,Xj+D1*dt/2,u(i)); 119 | D3 = Pendulum_Regul_Obs_Proj(t+dt/2,Xj+D2*dt/2,u(i)); 120 | D4 = Pendulum_Regul_Obs_Proj(t+dt,Xj+D3*dt,u(i)); 121 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 122 | X(:,i+1) = Xj; 123 | Time(i+1) = t + dt; 124 | i = i + 1; 125 | t = t + dt; 126 | end 127 | 128 | %% Plots 129 | figure; 130 | subplot(5,1,1);plot(Time,X(1,:),Time(1:end-1),Xh(1,:),'g'); 131 | title('Regulator With Observer Design Initial Conddition "0,0,10,5"and Smapling Time of "0.01"') 132 | xlabel('time(s)') 133 | ylabel('x(m)') 134 | legend('X','Xhat','location','northeast') 135 | 136 | subplot(5,1,2);plot(Time,X(2,:),Time(1:end-1),Xh(2,:),'g'); 137 | xlabel('time(s)') 138 | ylabel('xdot(m/s)') 139 | legend('X','Xhat','location','northeast') 140 | 141 | subplot(5,1,3);plot(Time,X(3,:),Time(1:end-1),Xh(3,:),'g'); 142 | xlabel('time(s)') 143 | ylabel('theta(rad)') 144 | legend('X','Xhat','location','northeast') 145 | 146 | subplot(5,1,4);plot(Time,X(4,:),Time(1:end-1),Xh(4,:),'g'); 147 | xlabel('time(s)') 148 | ylabel('thetadot(rad/s)') 149 | legend('X','Xhat','location','northeast') 150 | 151 | subplot(5,1,5);plot(Time(1:end-1),u); 152 | xlabel('time(s)') 153 | ylabel('ZOH Input') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Servo_Adding_Integrator/Pendulum_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_Add_Int_Proj(t,X,u,yr) 2 | global M_Cart m g l C A B 3 | 4 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 5 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | 7 | Xx = X(1:4); 8 | Xi = X(5); 9 | y = C*Xx; 10 | 11 | DXx = A*Xx + B*u; 12 | DXi = yr - y; 13 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Servo_Adding_Integrator/Project_Inverted_Pendulum_System_Servo_Adding_Integrator.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_Inverted_Pendulum_System_Servo_Adding_Integrator 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | h = 0.05; 12 | 13 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 14 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 15 | C = [1 0 0 0]; 16 | G = expm(A_Linear*h); 17 | H = (eye(4) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 18 | Gh = [G zeros(4,1);-C 1]; 19 | Hh = [H;0]; 20 | 21 | mu_dcon = [-3 -3 -2-2i -2+2i -4]; 22 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h) exp(mu_dcon(5)*h)]; 23 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 24 | disp(mu_ddis) 25 | 26 | %% Designing Controller Gain, Ackerman Method 27 | Mh_Discerete = [Hh Gh*Hh Gh^2*Hh Gh^3*Hh Gh^4*Hh]; 28 | r_Mh_Discerete = rank(Mh_Discerete); 29 | 30 | if r_Mh_Discerete == min(size(Mh_Discerete)) 31 | 32 | fprintf('The discerete time system is controllable and the rank of Mh is\n') 33 | disp(r_Mh_Discerete) 34 | 35 | K = acker(Gh,Hh,mu_ddis); 36 | 37 | fprintf('The Controller Gain "K" is\n') 38 | disp(K) 39 | 40 | else 41 | disp('The continous time system is unctrollable') 42 | end 43 | 44 | %% Simulation 45 | T =400; 46 | dt = 0.001; 47 | X0 = [0;0;0.17;0.08;0]; 48 | t = 0; 49 | Time(1) = t; 50 | k = 0; 51 | i = 1; 52 | X(:,i) = X0; 53 | while t < T 54 | Xj = X(:,i); 55 | 56 | if mod(i,floor(h/dt))==1 57 | k=k+1; 58 | ud(k)=-K*Xj; 59 | end 60 | 61 | yr(i) = 0.5*sign(sin(0.02*t)); 62 | u(:,i) = ud(k); 63 | D1 = Pendulum_Servo_Add_Int_Proj(t,Xj,u(i),yr(i)); 64 | D2 = Pendulum_Servo_Add_Int_Proj(t+dt/2,Xj+D1*dt/2,u(i),yr(i)); 65 | D3 = Pendulum_Servo_Add_Int_Proj(t+dt/2,Xj+D2*dt/2,u(i),yr(i)); 66 | D4 = Pendulum_Servo_Add_Int_Proj(t+dt,Xj+D3*dt,u(i),yr(i)); 67 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 68 | X(:,i+1) = Xj; 69 | Time(i+1) = t + dt; 70 | i = i+1; 71 | t = t + dt; 72 | end 73 | 74 | %% Plots 75 | figure; 76 | subplot(5,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'g'); 77 | title('Adding Integrator Servo Design Initial Conddition "0,0,0,0" and Smapling Time of "0.01"') 78 | xlabel('time(s)') 79 | ylabel('x(m)') 80 | legend('Y','Yr','location','northeast') 81 | 82 | subplot(5,1,2);plot(Time,X(2,:)); 83 | xlabel('time(s)') 84 | ylabel('xdot(m/s)') 85 | 86 | subplot(5,1,3);plot(Time,X(3,:)); 87 | xlabel('time(s)') 88 | ylabel('theta(rad)') 89 | 90 | subplot(5,1,4);plot(Time,X(4,:)); 91 | xlabel('time(s)') 92 | ylabel('thetadot(rad/s)') 93 | 94 | subplot(5,1,5);plot(Time(1:end-1),u); 95 | xlabel('time(s)') 96 | ylabel('ZOH Input') -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Servo_Adding_Integrator_Observer/Pendulum_Servo_Add_Int_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_Add_Int_Obs_Proj(t,X,u,yr) 2 | global C A B m g M_Cart l 3 | 4 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 5 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | 7 | Xx = X(1:4); 8 | Xi = X(5); 9 | y = C*Xx; 10 | 11 | DXx = A*Xx + B*u; 12 | DXi = yr - y; 13 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Discerete_Time_Inverted_Pendulum_System_Servo_Adding_Integrator_Observer/Project_Inverted_Pendulum_System_Servo_Adding_Integrator_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#2_Advanced_Control_Inverted_Pendulum_System_Servo_Adding_Integrator_Observer 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | h = 0.05; 12 | 13 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 14 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 15 | C = [1 0 0 0]; 16 | G = expm(A_Linear*h); 17 | H = (eye(4) + (A_Linear*h)/2 + ((A_Linear^2)*(h^2))/6 + ((A_Linear^3)*(h^3))/24)*h*B_Linear; 18 | Gh = [G zeros(4,1);-C 1]; 19 | Hh = [H;0]; 20 | 21 | mu_dcon = [-3 -3 -2-2i -2+2i -2]; 22 | mu_ddis = [exp(mu_dcon(1)*h) exp(mu_dcon(2)*h) exp(mu_dcon(3)*h) exp(mu_dcon(4)*h) exp(mu_dcon(5)*h)]; 23 | fprintf('The closed loop discerete time system desired eigen values must be placed at\n') 24 | disp(mu_ddis) 25 | 26 | muo_dcon = [-10 -10 -10 -10]; 27 | muo_ddis = [exp(muo_dcon(1)*h) exp(muo_dcon(2)*h) exp(muo_dcon(3)*h) exp(muo_dcon(4)*h)]; 28 | fprintf('The closed loop discerete time approximated system desired eigen values must be placed at\n') 29 | disp(muo_ddis) 30 | 31 | %% Designing Controller Gain, Ackerman Method 32 | Mh_Discerete = [Hh Gh*Hh Gh^2*Hh Gh^3*Hh Gh^4*Hh]; 33 | r_Mh_Discerete = rank(Mh_Discerete); 34 | 35 | if r_Mh_Discerete == min(size(Mh_Discerete)) 36 | 37 | fprintf('The discerete time system is controllable and the rank of Mh is\n') 38 | disp(r_Mh_Discerete) 39 | 40 | K = acker(Gh,Hh,mu_ddis); 41 | 42 | fprintf('The Controller Gain "K" is\n') 43 | disp(K) 44 | 45 | else 46 | disp('The continous time system is unctrollable') 47 | end 48 | %% Designing Observer Gain, Ackerman Method 49 | N_T_Contionous = [C' A_Linear'*C' (A_Linear')^2*C' (A_Linear')^3*C']; 50 | r_N_Contionous = rank(N_T_Contionous); 51 | N_T_Discerete = [C' G'*C' (G')^2*C' (G')^3*C']; 52 | r_N_Discerete = rank(N_T_Discerete); 53 | 54 | if r_N_Contionous == min(size(N_T_Contionous)) 55 | 56 | fprintf('The contiouns time system is Observable and the rank of N_Transpose is\n') 57 | disp(r_N_Contionous) 58 | 59 | else 60 | disp('The continous time system is unobservable') 61 | end 62 | 63 | if r_N_Discerete == min(size(N_T_Discerete)) 64 | 65 | fprintf('The discerete time system is Observable and the rank of N_Transpose is\n') 66 | disp(r_N_Discerete) 67 | 68 | Ko = acker(G',C',muo_ddis); 69 | Ko = Ko'; 70 | 71 | fprintf('The observer gain "Ko" is\n') 72 | disp(Ko) 73 | 74 | else 75 | disp('The discerete time system is unobservable') 76 | end 77 | 78 | %% Simulation 79 | T = 400; 80 | dt = 0.001; 81 | X0 = [0;0;0.017/2;0.008/2;0]; 82 | Xh0 = [0.001/2;0.01/2;0.01/2;0.001/2]; 83 | t = 0; 84 | Time(1) = t; 85 | k = 0; 86 | i = 1; 87 | X(:,i) = X0; 88 | Xh(:,1) = Xh0; 89 | 90 | while t < T 91 | 92 | Xj = X(:,i); 93 | y = C*Xj(1:4); 94 | if mod(i,floor(h/dt))==1 95 | 96 | k=k+1; 97 | ud(k)=-K(1:4)*Xh(:,k) - K(5)*Xj(5); 98 | yh = C*Xh(:,k); 99 | Xh(:,k+1) = G*Xh(:,k) + H*ud(k) + Ko*(y-yh); 100 | end 101 | 102 | u(:,i) = ud(k); 103 | Xhj = Xh(:,k); 104 | Xh(:,i) = Xhj; 105 | yr(i) = 0.5*sign(sin(0.02*t)); 106 | 107 | D1 = Pendulum_Servo_Add_Int_Obs_Proj(t,Xj,u(i),yr(i)); 108 | D2 = Pendulum_Servo_Add_Int_Obs_Proj(t+dt/2,Xj+D1*dt/2,u(i),yr(i)); 109 | D3 = Pendulum_Servo_Add_Int_Obs_Proj(t+dt/2,Xj+D2*dt/2,u(i),yr(i)); 110 | D4 = Pendulum_Servo_Add_Int_Obs_Proj(t+dt,Xj+D3*dt,u(i),yr(i)); 111 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 112 | X(:,i+1) = Xj; 113 | Time(i+1) = t + dt; 114 | i = i + 1; 115 | t = t + dt; 116 | end 117 | 118 | %% Plots 119 | figure; 120 | subplot(5,1,1);plot(Time,X(1,:),Time(1:end-1),Xh(1,:),'g',Time(1:end-1),yr,'r'); 121 | title('Servo With Observer Design and Smapling Time of "0.05"') 122 | xlabel('time(s)') 123 | ylabel('x(m)') 124 | legend('X','Xhat','Yr','location','northeast') 125 | 126 | subplot(5,1,2);plot(Time,X(2,:),Time(1:end-1),Xh(2,:),'g'); 127 | xlabel('time(s)') 128 | ylabel('xdot(m/s)') 129 | legend('X','Xhat','location','northeast') 130 | 131 | subplot(5,1,3);plot(Time,X(3,:),Time(1:end-1),Xh(3,:),'g'); 132 | xlabel('time(s)') 133 | ylabel('theta(rad)') 134 | legend('X','Xhat','location','northeast') 135 | 136 | subplot(5,1,4);plot(Time,X(4,:),Time(1:end-1),Xh(4,:),'g'); 137 | xlabel('time(s)') 138 | ylabel('thetadot(rad/s)') 139 | legend('X','Xhat','location','northeast') 140 | 141 | subplot(5,1,5);plot(Time(1:end-1),u); 142 | xlabel('time(s)') 143 | ylabel('ZOH Input') -------------------------------------------------------------------------------- /Effect_of_Data_Transmition_Rate_on_Persuit_Guidance/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#3 2 | 3 | ro = 1.225; %kg/m^3% 4 | S = 0.1; %m^2% 5 | g = 0; %m/s^2% 6 | aty = 0; %m/s^2%d 7 | atz = 0; %m/s^2% 8 | G_p = 7.5; 9 | T_1 = 0; 10 | T_2 = 0; 11 | T = 0.2; 12 | 13 | Psi_m0 = 10*pi/180; 14 | Theta_m0 = 24*pi/180; 15 | V_m0B = [1000;0;0]; 16 | R_Psi = [cos(Psi_m0) sin(Psi_m0) 0;-sin(Psi_m0) cos(Psi_m0) 0;0 0 1]; 17 | R_Theta = [cos(Theta_m0) 0 -sin(Theta_m0);0 1 0;sin(Theta_m0) 0 cos(Theta_m0)]; 18 | R_BI = R_Theta*R_Psi; 19 | R_IB = R_BI'; 20 | V_m0I = V_m0B; -------------------------------------------------------------------------------- /Effect_of_Data_Transmition_Rate_on_Persuit_Guidance/Problem_1_Part_G.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Data_Transmition_Rate_on_Persuit_Guidance/Problem_1_Part_G.slx -------------------------------------------------------------------------------- /Effect_of_Data_Transmition_Rate_on_Persuit_Guidance/Problem_1_Part_G.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Data_Transmition_Rate_on_Persuit_Guidance/Problem_1_Part_G.slxc -------------------------------------------------------------------------------- /Effect_of_Data_Transmition_Rate_on_Persuit_Guidance/delT_Variable_Part_G.m: -------------------------------------------------------------------------------- 1 | %% delT_Variable_Part_G_Guidance_and_Navigation_HW#3 2 | clc 3 | clear all 4 | delT = 0.01; 5 | i = 1; 6 | for delT = 0.01:0.01:0.2 7 | sim('Problem_1_Part_G') 8 | MD1(i) = MD(end); 9 | i = i+1; 10 | end 11 | 12 | %% Miss_Distance 13 | delT = 0.01:0.01:0.2; 14 | figure('Name','MD-delT Diagram') 15 | plot(delT,MD1) 16 | grid on 17 | title('Miss Distance Variation vs Zero Order Hold Acceleration Command') 18 | xlabel('delT (s)') 19 | ylabel('MD (m)') 20 | -------------------------------------------------------------------------------- /Effect_of_Guidance_Time_Constant_on_Persuit_Guidance/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#3 2 | 3 | ro = 1.225; %kg/m^3% 4 | S = 0.1; %m^2% 5 | g = 0; %m/s^2% 6 | aty = 0; %m/s^2%d 7 | atz = 0; %m/s^2% 8 | G_p = 7.5; 9 | T_1 = 0; 10 | T_2 = 0; 11 | R_min = 10; 12 | 13 | Psi_m0 = 10*pi/180; 14 | Theta_m0 = 24*pi/180; 15 | V_m0B = [1000;0;0]; 16 | R_Psi = [cos(Psi_m0) sin(Psi_m0) 0;-sin(Psi_m0) cos(Psi_m0) 0;0 0 1]; 17 | R_Theta = [cos(Theta_m0) 0 -sin(Theta_m0);0 1 0;sin(Theta_m0) 0 cos(Theta_m0)]; 18 | R_BI = R_Theta*R_Psi; 19 | R_IB = R_BI'; 20 | V_m0I = V_m0B; -------------------------------------------------------------------------------- /Effect_of_Guidance_Time_Constant_on_Persuit_Guidance/Problem_1_Part_C.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Guidance_Time_Constant_on_Persuit_Guidance/Problem_1_Part_C.slx -------------------------------------------------------------------------------- /Effect_of_Guidance_Time_Constant_on_Persuit_Guidance/Problem_1_Part_C.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Guidance_Time_Constant_on_Persuit_Guidance/Problem_1_Part_C.slxc -------------------------------------------------------------------------------- /Effect_of_Guidance_Time_Constant_on_Persuit_Guidance/T_Variable_Part_C.m: -------------------------------------------------------------------------------- 1 | %% T_Variable_Part_C_Guidance_and_Navigation_HW#3 2 | clc 3 | clear all 4 | T = 0; 5 | i = 1; 6 | for T = 0:0.01:1 7 | sim('Problem_1_Part_C') 8 | MD1(i) = MD(end); 9 | CE1(i) = CE(end); 10 | i = i+1; 11 | end 12 | 13 | %% Miss_Distance 14 | T = 0:0.01:1; 15 | figure('Name','MD-T Diagram') 16 | plot(T,MD1) 17 | grid on 18 | title('Miss Distance Variation vs Time Constant Variation') 19 | xlabel('Guidance Time Constant (sec)') 20 | ylabel('MD (m)') 21 | 22 | %% Control_Effort 23 | figure('Name','CE-Time Diagram') 24 | plot(T,CE1) 25 | grid on 26 | title('Control Effort Variation vs Time Constant Variation') 27 | xlabel('Guidance Time Constant (sec)') 28 | ylabel('CE (m/s)') 29 | -------------------------------------------------------------------------------- /Effect_of_Look_Angle_on_Persuit_Guidance/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#3 2 | 3 | ro = 1.225; %kg/m^3% 4 | S = 0.1; %m^2% 5 | g = 0; %m/s^2% 6 | aty = 0; %m/s^2%d 7 | atz = 0; %m/s^2% 8 | G_p = 7.5; 9 | T_1 = 0; 10 | T_2 = 0; 11 | T = 0.2; 12 | 13 | Psi_m0 = 10*pi/180; 14 | Theta_m0 = 24*pi/180; 15 | V_m0B = [1000;0;0]; 16 | R_Psi = [cos(Psi_m0) sin(Psi_m0) 0;-sin(Psi_m0) cos(Psi_m0) 0;0 0 1]; 17 | R_Theta = [cos(Theta_m0) 0 -sin(Theta_m0);0 1 0;sin(Theta_m0) 0 cos(Theta_m0)]; 18 | R_BI = R_Theta*R_Psi; 19 | R_IB = R_BI'; 20 | V_m0I = V_m0B; -------------------------------------------------------------------------------- /Effect_of_Look_Angle_on_Persuit_Guidance/Problem_1_Part_F.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Look_Angle_on_Persuit_Guidance/Problem_1_Part_F.slx -------------------------------------------------------------------------------- /Effect_of_Look_Angle_on_Persuit_Guidance/Problem_1_Part_F.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Look_Angle_on_Persuit_Guidance/Problem_1_Part_F.slxc -------------------------------------------------------------------------------- /Effect_of_Look_Angle_on_Persuit_Guidance/emax_Variable_Part_F.m: -------------------------------------------------------------------------------- 1 | %% emax_Variable_Part_F_Guidance_and_Navigation_HW#3 2 | clc 3 | clear all 4 | e_max = 5; 5 | i = 1; 6 | for e_max = 5:1:20 7 | sim('Problem_1_Part_F') 8 | MD1(i) = MD(end); 9 | i = i+1; 10 | end 11 | 12 | %% Miss_Distance 13 | e_max = 5:1:20; 14 | figure('Name','MD-emax Diagram') 15 | plot(e_max,MD1) 16 | grid on 17 | title('Miss Distance Variation vs Maximum Look Angle') 18 | xlabel('emax (deg)') 19 | ylabel('MD (m)') 20 | -------------------------------------------------------------------------------- /Effect_of_Saturated_Acceleration_on_Persuit_Guidance/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#3 2 | 3 | ro = 1.225; %kg/m^3% 4 | S = 0.1; %m^2% 5 | g = 0; %m/s^2% 6 | aty = 0; %m/s^2%d 7 | atz = 0; %m/s^2% 8 | G_p = 7.5; 9 | T = 0.2; 10 | T_1 = 0; 11 | T_2 = 0; 12 | R_min = 10; 13 | 14 | Psi_m0 = 10*pi/180; 15 | Theta_m0 = 24*pi/180; 16 | V_m0B = [1000;0;0]; 17 | R_Psi = [cos(Psi_m0) sin(Psi_m0) 0;-sin(Psi_m0) cos(Psi_m0) 0;0 0 1]; 18 | R_Theta = [cos(Theta_m0) 0 -sin(Theta_m0);0 1 0;sin(Theta_m0) 0 cos(Theta_m0)]; 19 | R_BI = R_Theta*R_Psi; 20 | R_IB = R_BI'; 21 | V_m0I = V_m0B; -------------------------------------------------------------------------------- /Effect_of_Saturated_Acceleration_on_Persuit_Guidance/Problem_1_Part_D.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Saturated_Acceleration_on_Persuit_Guidance/Problem_1_Part_D.slx -------------------------------------------------------------------------------- /Effect_of_Saturated_Acceleration_on_Persuit_Guidance/Problem_1_Part_D.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Saturated_Acceleration_on_Persuit_Guidance/Problem_1_Part_D.slxc -------------------------------------------------------------------------------- /Effect_of_Saturated_Acceleration_on_Persuit_Guidance/alim_Variable_Part_D.m: -------------------------------------------------------------------------------- 1 | %% alim_Variable_Part_D_Guidance_and_Navigation_HW#3 2 | clc 3 | clear all 4 | alim = 10; 5 | i = 1; 6 | for alim = 10:1:100 7 | sim('Problem_1_Part_D') 8 | MD1(i) = MD(end); 9 | CE1(i) = CE(end); 10 | i = i+1; 11 | end 12 | 13 | %% Miss_Distance 14 | alim = 10:1:100; 15 | figure('Name','MD-alim Diagram') 16 | plot(alim,MD1) 17 | grid on 18 | title('Miss Distance Variation vs Maximum Allowable Accelertion') 19 | xlabel('Limited Acceleration (G)') 20 | ylabel('MD (m)') 21 | 22 | %% Control_Effort 23 | figure('Name','CE-alim Diagram') 24 | plot(alim,CE1) 25 | grid on 26 | title('Control Effort Variation vs Maximum Allowable Accelertion') 27 | xlabel('Limited Acceleration (m/s^2)') 28 | ylabel('CE (m/s)') 29 | -------------------------------------------------------------------------------- /Effect_of_Seeker_Range_on_Persuit_Guidance/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#3 2 | 3 | ro = 1.225; %kg/m^3% 4 | S = 0.1; %m^2% 5 | g = 0; %m/s^2% 6 | aty = 0; %m/s^2%d 7 | atz = 0; %m/s^2% 8 | G_p = 7.5; 9 | T_1 = 0; 10 | T_2 = 0; 11 | T = 0.2; 12 | 13 | Psi_m0 = 10*pi/180; 14 | Theta_m0 = 24*pi/180; 15 | V_m0B = [1000;0;0]; 16 | R_Psi = [cos(Psi_m0) sin(Psi_m0) 0;-sin(Psi_m0) cos(Psi_m0) 0;0 0 1]; 17 | R_Theta = [cos(Theta_m0) 0 -sin(Theta_m0);0 1 0;sin(Theta_m0) 0 cos(Theta_m0)]; 18 | R_BI = R_Theta*R_Psi; 19 | R_IB = R_BI'; 20 | V_m0I = V_m0B; -------------------------------------------------------------------------------- /Effect_of_Seeker_Range_on_Persuit_Guidance/Problem_1_Part_E.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Seeker_Range_on_Persuit_Guidance/Problem_1_Part_E.slx -------------------------------------------------------------------------------- /Effect_of_Seeker_Range_on_Persuit_Guidance/Problem_1_Part_E.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Effect_of_Seeker_Range_on_Persuit_Guidance/Problem_1_Part_E.slxc -------------------------------------------------------------------------------- /Effect_of_Seeker_Range_on_Persuit_Guidance/Rmin_Variable_Part_E.m: -------------------------------------------------------------------------------- 1 | %% Rmin_Variable_Part_E_Guidance_and_Navigation_HW#3 2 | clc 3 | clear all 4 | R_min = 100; 5 | i = 1; 6 | for R_min = 100:10:2000 7 | sim('Problem_1_Part_E') 8 | MD1(i) = MD(end); 9 | i = i+1; 10 | end 11 | 12 | %% Miss_Distance 13 | R_min = 100:10:2000; 14 | figure('Name','MD-Rmin Diagram') 15 | plot(R_min,MD1) 16 | grid on 17 | title('Miss Distance Variation vs Minimum Allowable Distance Between Missile and Target') 18 | xlabel('Rmin (m)') 19 | ylabel('MD (m)') 20 | -------------------------------------------------------------------------------- /Inverted_Pendulum_System_LQR_Design/Pendulum_LQR.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_LQR(t,X,u) 2 | 3 | global M_Cart m g l 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX=A*X+B*u; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_LQR_Design/Project_Inverted_Pendulum_System_LQR_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Designing_LQR_Controller_For_Inverted_Pendulum_System_B10_21 2 | clc 3 | clear 4 | global M_Cart m g l 5 | 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | Q = 5*eye(4); 15 | R = 20; 16 | 17 | %% Designing Controller Gain, LQR Method 18 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 19 | r_M = rank(M); 20 | if r_M == min(size(M)) 21 | fprintf('The system is controllable and the rank of M is\n') 22 | disp(r_M) 23 | 24 | [K_reg,P,E]=lqr(A_Linear,B_Linear,Q,R) 25 | 26 | fprintf('The Controller Gain "K" is\n') 27 | disp(K_reg) 28 | else 29 | disp('The System is Unctrollable') 30 | end 31 | 32 | %% Simulation 33 | T = 10; 34 | dt = 0.01; 35 | X0 = [0;0;0.349066;0.174533]; 36 | t = 0; 37 | X(:,1) = X0; 38 | Time(1) = t; 39 | k = 1; 40 | while t < T 41 | Xj = X(:,k); 42 | u = -K_reg*Xj; 43 | D1 = Pendulum_LQR(t,Xj,u); 44 | D2 = Pendulum_LQR(t+dt/2,Xj+D1*dt/2,u); 45 | D3 = Pendulum_LQR(t+dt/2,Xj+D2*dt/2,u); 46 | D4 = Pendulum_LQR(t+dt,Xj+D3*dt,u); 47 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 48 | X(:,k+1) = Xj; 49 | Time(k+1) = t + dt; 50 | k = k + 1; 51 | t = t + dt; 52 | end 53 | 54 | %% Plots 55 | figure; 56 | subplot(4,1,1);plot(Time,X(1,:)); 57 | title('LQR Controller Design Initial Conddition "0,0,20,10" and R equals to "20"') 58 | xlabel('time(s)') 59 | ylabel('x(m)') 60 | 61 | subplot(4,1,2);plot(Time,X(2,:)); 62 | xlabel('time(s)') 63 | ylabel('xdot(m/s)') 64 | 65 | subplot(4,1,3);plot(Time,X(3,:)); 66 | xlabel('time(s)') 67 | ylabel('theta(rad)') 68 | 69 | subplot(4,1,4);plot(Time,X(4,:)); 70 | xlabel('time(s)') 71 | ylabel('thetadot(rad/s)') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Regulator_Design/Pendulum_Regul_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Regul_Proj(t,X,u) 2 | 3 | global M_Cart m g l A B 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Regulator_Design/Project_Inverted_Pendulum_System_Regulator_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Regulator_Design 2 | clc 3 | clear 4 | 5 | global M_Cart m g l 6 | %% Designing Controller Gain, Ackerman Method 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | C_Ytheta = [0 0 1 0]; 16 | 17 | 18 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 19 | r_M = rank(M); 20 | if r_M == min(size(M)) 21 | fprintf('The system is controllable and the rank of M is\n') 22 | disp(r_M) 23 | 24 | mu_d = [-3 -3 -2+2i -2-2i]; %% Desired Eigenvalues 25 | K_Reg = acker(A_Linear,B_Linear,mu_d); 26 | 27 | fprintf('The Controller Gain "K" is\n') 28 | disp(K_Reg) 29 | else 30 | disp('The System is Unctrollable') 31 | end 32 | 33 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 34 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 35 | r_N_T_Yx = rank(N_T_Yx); 36 | if r_N_T_Yx == min(size(N_T_Yx)) 37 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 38 | disp(r_N_T_Yx) 39 | 40 | muo_d = [-10 -10 -10 -10]; 41 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 42 | Ko_Yx = Ko_T_Yx'; 43 | 44 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 45 | disp(Ko_Yx) 46 | else 47 | disp('The system is unobservable when it observes (x)') 48 | end 49 | 50 | %% Designing Observer Gain When System Observes X3=theta, Ackerman Method 51 | N_T_Ytheta = [C_Ytheta' A_Linear'*C_Ytheta' (A_Linear')^2*C_Ytheta' (A_Linear')^3*C_Ytheta']; %% N tranpose matrix when system observs X3=theta 52 | r_N_T_Ytheta = rank(N_T_Ytheta); 53 | if r_N_T_Ytheta == min(size(N_T_Ytheta)) 54 | fprintf('When system observes (theta), it is Observable and the rank of N_Transpose is\n') 55 | disp(r_N_T_Ytheta) 56 | 57 | muo_d = [-10 -10 -10 -10]; 58 | Ko_T_Ytheta = acker(A_Linear',C_Ytheta',muo_d); 59 | Ko_Ytheta = Ko_T_Ytheta'; 60 | 61 | fprintf('The Observer Gain "Ko" When System Observes (theta) is\n') 62 | disp(Ko_Ytheta) 63 | else 64 | disp('The system is unobservable when it observes (theta)') 65 | end 66 | 67 | %% Simulation 68 | T = 5; 69 | dt = 0.01; 70 | X0 = [0;0;0.349066;0.174533]; 71 | t = 0; 72 | X(:,1) = X0; 73 | Time(1) = t; 74 | k = 1; 75 | while t < T 76 | Xj = X(:,k); 77 | u = -K_Reg*Xj; 78 | D1 = Pendulum_Regul_Proj(t,Xj,u); 79 | D2 = Pendulum_Regul_Proj(t+dt/2,Xj+D1*dt/2,u); 80 | D3 = Pendulum_Regul_Proj(t+dt/2,Xj+D2*dt/2,u); 81 | D4 = Pendulum_Regul_Proj(t+dt,Xj+D3*dt,u); 82 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 83 | X(:,k+1) = Xj; 84 | Time(k+1) = t + dt; 85 | k = k+1; 86 | t = t + dt; 87 | end 88 | 89 | %% Plots 90 | figure; 91 | subplot(4,1,1);plot(Time,X(1,:)); 92 | title('Regulator Controller Design Initial Conddition "0,0,20,10"') 93 | xlabel('time(s)') 94 | ylabel('x(m)') 95 | 96 | subplot(4,1,2);plot(Time,X(2,:)); 97 | xlabel('time(s)') 98 | ylabel('xdot(m/s)') 99 | 100 | subplot(4,1,3);plot(Time,X(3,:)); 101 | xlabel('time(s)') 102 | ylabel('theta(rad)') 103 | 104 | subplot(4,1,4);plot(Time,X(4,:)); 105 | xlabel('time(s)') 106 | ylabel('thetadot(rad/s)') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Regulator_Full_Observer_Design/Pendulum_Full_Obser_Regual_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Pendulum_Full_Obser_Regual_Proj(t,Xh,u,y) 2 | global A B C_Yx Ko_Yx 3 | 4 | 5 | yh = C_Yx*Xh; 6 | DXh = A*Xh + B*u + Ko_Yx*(y-yh); -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Regulator_Full_Observer_Design/Pendulum_Regul_FO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Regul_FO_Obs_Proj(t,X,u) 2 | 3 | global A B M_Cart m g l 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Regulator_Full_Observer_Design/Project_Inverted_Pendulum_System_Regulator_Full_Observer_Design.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Regulator_Full_Observer_Design 2 | clc 3 | clear 4 | 5 | global M_Cart m g l Ko_Yx C_Yx 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | 16 | %% Designing Controller Gain, Ackerman Method 17 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 18 | r_M = rank(M); 19 | if r_M == min(size(M)) 20 | fprintf('The system is controllable and the rank of M is\n') 21 | disp(r_M) 22 | 23 | mu_d = [-3 -3 -2+2i -2-2i]; %% Desired Eigenvalues 24 | K_Reg = acker(A_Linear,B_Linear,mu_d); 25 | 26 | fprintf('The Controller Gain "K" is\n') 27 | disp(K_Reg) 28 | else 29 | disp('The System is Unctrollable') 30 | end 31 | 32 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 33 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 34 | r_N_T_Yx = rank(N_T_Yx); 35 | if r_N_T_Yx == min(size(N_T_Yx)) 36 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 37 | disp(r_N_T_Yx) 38 | 39 | muo_d = [-10 -10 -10 -10]; 40 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 41 | Ko_Yx = Ko_T_Yx'; 42 | 43 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 44 | disp(Ko_Yx) 45 | else 46 | disp('The system is unobservable when it observes (x)') 47 | end 48 | 49 | %% Simulation 50 | T = 5; 51 | dt = 0.001; 52 | X0 = [0;0;0.349066;0.174533]; 53 | Xh0 = [0.01;0.01;0.1;0.1]; 54 | t = 0; 55 | X(:,1) = X0; 56 | Xh(:,1) = Xh0; 57 | Time(1) = t; 58 | k = 1; 59 | while t < T 60 | Xj = X(:,k); 61 | Xhj = Xh(:,k); 62 | y = C_Yx*Xj; 63 | u = -K_Reg*Xhj; 64 | D1 = Pendulum_Regul_FO_Obs_Proj(t,Xj,u); 65 | D2 = Pendulum_Regul_FO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u); 66 | D3 = Pendulum_Regul_FO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u); 67 | D4 = Pendulum_Regul_FO_Obs_Proj(t+dt,Xj+D3*dt,u); 68 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 69 | X(:,k+1) = Xj; 70 | O1 = Pendulum_Full_Obser_Regual_Proj(t,Xhj,u,y); 71 | O2 = Pendulum_Full_Obser_Regual_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 72 | O3 = Pendulum_Full_Obser_Regual_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 73 | O4 = Pendulum_Full_Obser_Regual_Proj(t+dt,Xhj+O3*dt,u,y); 74 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 75 | Xh(:,k+1) = Xhj; 76 | Time(k+1) = t + dt; 77 | k = k + 1; 78 | t = t + dt; 79 | end 80 | 81 | %% Plots 82 | figure; 83 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g'); 84 | title('Regulator With Full Order ObserverController Design Initial Conddition "0,0,20,10"') 85 | xlabel('time(s)') 86 | ylabel('x(m)') 87 | legend('X','Xhat','location','northeast') 88 | 89 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(1,:),'g'); 90 | xlabel('time(s)') 91 | ylabel('xdot(m/s)') 92 | legend('X','Xhat','location','northeast') 93 | 94 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(1,:),'g'); 95 | xlabel('time(s)') 96 | ylabel('theta(rad)') 97 | legend('X','Xhat','location','northeast') 98 | 99 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(1,:),'g'); 100 | xlabel('time(s)') 101 | ylabel('thetadot(rad/s)') 102 | legend('X','Xhat','location','northeast') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator/Pendulum_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_Add_Int_Proj(t,X,u,yr) 2 | global M_Cart m g l C_Yx A B 3 | 4 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 5 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | 7 | Xx = X(1:4); 8 | Xi = X(5); 9 | y = C_Yx*Xx; 10 | 11 | DXx = A*Xx + B*u; 12 | DXi = yr - y; 13 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator/Project_Inverted_Pendulum_System_Servo_Adding_Integrator.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Servo_Adding_Integrator 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C_Yx 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 16 | Bh_Linear = [B_Linear;0]; 17 | 18 | %% Designing Controller Gain, Ackerman Method 19 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear]; 20 | r_Mh = rank(Mh); 21 | if r_Mh == min(size(Mh)) 22 | fprintf('The system is controllable and the rank of Mh is\n') 23 | disp(r_Mh) 24 | 25 | mu_d = [-2+2i -2-2i -3 -3 -2]; %% Desired Eigenvalues 26 | Kh_srv = acker(Ah_Linear,Bh_Linear,mu_d); 27 | 28 | fprintf('The Controller Gain "K" is\n') 29 | disp(Kh_srv) 30 | else 31 | disp('The System is Unctrollable') 32 | end 33 | 34 | %% Simulation 35 | T = 60; 36 | dt = 0.01; 37 | X0 = [0;0;0.349066;0.174533;0]; 38 | t = 0; 39 | X(:,1) = X0; 40 | Time(1) = t; 41 | k = 1; 42 | while t < T 43 | Xj = X(:,k); 44 | yr(k) = 0.5*sign(sin(0.2*t)); 45 | u = -Kh_srv*Xj; 46 | D1 = Pendulum_Servo_Add_Int_Proj(t,Xj,u,yr(k)); 47 | D2 = Pendulum_Servo_Add_Int_Proj(t+dt/2,Xj+D1*dt/2,u,yr(k)); 48 | D3 = Pendulum_Servo_Add_Int_Proj(t+dt/2,Xj+D2*dt/2,u,yr(k)); 49 | D4 = Pendulum_Servo_Add_Int_Proj(t+dt,Xj+D3*dt,u,yr(k)); 50 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 51 | X(:,k+1) = Xj; 52 | Time(k+1) = t + dt; 53 | k = k + 1; 54 | t = t + dt; 55 | end 56 | 57 | %% Plots 58 | figure; 59 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'g'); 60 | title('Adding Integrator Servo Design Initial Conddition "0,0,10,5"') 61 | xlabel('time(s)') 62 | ylabel('x(m)') 63 | legend('Y','Yr','location','northeast') 64 | 65 | subplot(4,1,2);plot(Time,X(2,:)); 66 | xlabel('time(s)') 67 | ylabel('xdot(m/s)') 68 | 69 | subplot(4,1,3);plot(Time,X(3,:)); 70 | xlabel('time(s)') 71 | ylabel('theta(rad)') 72 | 73 | subplot(4,1,4);plot(Time,X(4,:)); 74 | xlabel('time(s)') 75 | ylabel('thetadot(rad/s)') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator_Full_Order_Observer/Pendulum_Full_Obser_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Pendulum_Full_Obser_Servo_Add_Int_Proj(t,Xh,u,y) 2 | global Ko_Yx A B C_Yx 3 | 4 | yh = C_Yx*Xh; 5 | DXh = A*Xh + B*u + Ko_Yx*(y-yh); -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator_Full_Order_Observer/Pendulum_Servo_Add_Int_FO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_Add_Int_FO_Obs_Proj(t,X,u,yr) 2 | global C_Yx A B m g M_Cart l 3 | 4 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 5 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | 7 | Xx = X(1:4); 8 | Xi = X(5); 9 | y = C_Yx*Xx; 10 | 11 | DXx = A*Xx + B*u; 12 | DXi = yr - y; 13 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator_Full_Order_Observer/Project_Inverted_Pendulum_System_Servo_Adding_Integrator_FO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Servo_Adding_Integrator_Full_Order_Observer 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C_Yx 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 16 | Bh_Linear = [B_Linear;0]; 17 | 18 | %% Designing Controller Gain, Ackerman Method 19 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear]; 20 | r_Mh = rank(Mh); 21 | if r_Mh == min(size(Mh)) 22 | fprintf('The system is controllable and the rank of Mh is\n') 23 | disp(r_Mh) 24 | 25 | mu_d = [-2+2i -2-2i -3 -3 -2]; %% Desired Eigenvalues 26 | Kh_srv = acker(Ah_Linear,Bh_Linear,mu_d); 27 | 28 | fprintf('The Controller Gain "K" is\n') 29 | disp(Kh_srv) 30 | else 31 | disp('The System is Unctrollable') 32 | end 33 | 34 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 35 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 36 | r_N_T_Yx = rank(N_T_Yx); 37 | if r_N_T_Yx == min(size(N_T_Yx)) 38 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 39 | disp(r_N_T_Yx) 40 | 41 | muo_d = [-10 -10 -10 -10]; 42 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 43 | Ko_Yx = Ko_T_Yx'; 44 | 45 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 46 | disp(Ko_Yx) 47 | else 48 | disp('The system is unobservable when it observes (x)') 49 | end 50 | 51 | %% Simulation 52 | T = 60; 53 | dt = 0.001; 54 | X0 = [0;0;0.349066;0.174533;0]; 55 | Xh0 = [0.01;0.01;0.1;0.1]; 56 | t = 0; 57 | X(:,1) = X0; 58 | Xh(:,1) = Xh0; 59 | Time(1) = t; 60 | k = 1; 61 | while t < T 62 | Xj = X(:,k); 63 | Xhj = Xh(:,k); 64 | y = C_Yx*Xj(1:4); 65 | yr(k) = 0.5*sign(sin(0.2*t)); 66 | u = -Kh_srv(1:4)*Xhj - Kh_srv(5)*Xj(5); 67 | D1 = Pendulum_Servo_Add_Int_FO_Obs_Proj(t,Xj,u,yr(k)); 68 | D2 = Pendulum_Servo_Add_Int_FO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u,yr(k)); 69 | D3 = Pendulum_Servo_Add_Int_FO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u,yr(k)); 70 | D4 = Pendulum_Servo_Add_Int_FO_Obs_Proj(t+dt,Xj+D3*dt,u,yr(k)); 71 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 72 | X(:,k+1) = Xj; 73 | O1 = Pendulum_Full_Obser_Servo_Add_Int_Proj(t,Xhj,u,y); 74 | O2 = Pendulum_Full_Obser_Servo_Add_Int_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 75 | O3 = Pendulum_Full_Obser_Servo_Add_Int_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 76 | O4 = Pendulum_Full_Obser_Servo_Add_Int_Proj(t+dt,Xhj+O3*dt,u,y); 77 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 78 | Xh(:,k+1) = Xhj; 79 | 80 | Time(k+1) = t+dt; 81 | k = k+1; 82 | t = t + dt; 83 | end 84 | 85 | %% Plots 86 | figure; 87 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g',Time(1:end-1),yr,'r'); 88 | title('Integrator Servo Design with Observer Initial Conddition "0,0,10,5"') 89 | xlabel('time(s)') 90 | ylabel('x(m)') 91 | legend('X','Xh','Yr','location','northeast') 92 | 93 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(2,:),'g'); 94 | xlabel('time(s)') 95 | ylabel('xdot(m/s)') 96 | legend('X','Xh','location','northeast') 97 | 98 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(3,:),'g'); 99 | xlabel('time(s)') 100 | ylabel('theta(rad)') 101 | legend('X','Xh','location','northeast') 102 | 103 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(4,:),'g'); 104 | xlabel('time(s)') 105 | ylabel('thetadot(rad/s)') 106 | legend('X','Xh','location','northeast') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator_Minimum_Order_Observer/Pendulum_Min_Obser_Servo_Add_Int_Proj.m: -------------------------------------------------------------------------------- 1 | function DEth=Pendulum_Min_Obser_Servo_Add_Int_Proj(t,Eth,u,y) 2 | global Aaa Aab Aba Abb Ba Bb Ko_Yx 3 | 4 | 5 | 6 | DEth=Abb*Eth + Abb*Ko_Yx*y + Aba*y + Bb*u + Ko_Yx*(-Aaa*y - Aab*Eth - Aab*Ko_Yx*y - Ba*u); -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator_Minimum_Order_Observer/Pendulum_Servo_Add_Int_MO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_Add_Int_MO_Obs_Proj(t,X,u,yr) 2 | global C_Yx A B Aaa Aab Aba Abb Ba Bb l m g M_Cart 3 | 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | Aaa = A(1); 8 | Aab = A(1,2:4); 9 | Aba = A(2:4,1); 10 | Abb = [A(2,2:4);A(3,2:4);A(4,2:4)]; 11 | Ba = B(1); 12 | Bb = B(2:4,1); 13 | 14 | Xx = X(1:4); 15 | Xi = X(5); 16 | y = C_Yx*Xx; 17 | 18 | DXx = A*Xx + B*u; 19 | DXi = yr - y; 20 | DX = [DXx;DXi]; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Adding_Integrator_Minimum_Order_Observer/Project_Inverted_Pendulum_System_Servo_Adding_Integrator_MO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Servo_Adding_Integrator_Minimum_Order_Observer 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C_Yx Ko_Yx 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 16 | Bh_Linear = [B_Linear;0]; 17 | Aaa_Linear = A_Linear(1); 18 | Aab_Linear = A_Linear(1,2:4); 19 | Aba_Linear = A_Linear(2:4,1); 20 | Abb_Linear = [A_Linear(2,2:4);A_Linear(3,2:4);A_Linear(4,2:4)]; 21 | Ba_Linear = B_Linear(1); 22 | Bb_Linear = B_Linear(2:4,1); 23 | %% Designing Controller Gain, Ackerman Method 24 | Mh = [Bh_Linear Ah_Linear*Bh_Linear Ah_Linear^2*Bh_Linear Ah_Linear^3*Bh_Linear Ah_Linear^4*Bh_Linear]; 25 | r_Mh = rank(Mh); 26 | if r_Mh == min(size(Mh)) 27 | fprintf('The system is controllable and the rank of Mh is\n') 28 | disp(r_Mh) 29 | 30 | mu_d = [-2+2i -2-2i -3 -3 -2]; %% Desired Eigenvalues 31 | Kh_srv = acker(Ah_Linear,Bh_Linear,mu_d); 32 | 33 | fprintf('The Controller Gain "K" is\n') 34 | disp(Kh_srv) 35 | else 36 | disp('The System is Unctrollable') 37 | end 38 | 39 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 40 | N_T_Yx = [Aab_Linear' Abb_Linear'*Aab_Linear' (Abb_Linear')^2*Aab_Linear']; %% N tranpose matrix when system observs X1=x 41 | r_N_T_Yx = rank(N_T_Yx); 42 | if r_N_T_Yx == min(size(N_T_Yx)) 43 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 44 | disp(r_N_T_Yx) 45 | 46 | muo_d = [-10 -10 -10]; 47 | Ko_T_Yx = acker(Abb_Linear',Aab_Linear',muo_d); 48 | Ko_Yx = Ko_T_Yx'; 49 | 50 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 51 | disp(Ko_Yx) 52 | else 53 | disp('The system is unobservable when it observes (x)') 54 | end 55 | 56 | %% Simulation 57 | T = 60; 58 | dt = 0.0001; 59 | X0 = [0;0;0.349066;0.174533;0]; 60 | Xbh0 = [0.01;0.1;0.1]; 61 | Eth0 = Xbh0 - Ko_Yx*X0(1); 62 | t = 0; 63 | X(:,1) = X0; 64 | Xbh(:,1) = Xbh0; 65 | Eth(:,1) = Eth0; 66 | Time(1) = t; 67 | k = 1; 68 | while t < T 69 | Xj = X(:,k); 70 | Ethj = Eth(:,k); 71 | Xbhj = Ethj + Ko_Yx*Xj(1); 72 | y = C_Yx*Xj(1:4); %%%% y=Xa=X1; 73 | yr(k) = 0.5*sign(sin(0.2*t)); 74 | u = -Kh_srv(1)*Xj(1) - Kh_srv(2:4)*Xbhj - Kh_srv(5)*Xj(5); 75 | D1 = Pendulum_Servo_Add_Int_MO_Obs_Proj(t,Xj,u,yr(k)); 76 | D2 = Pendulum_Servo_Add_Int_MO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u,yr(k)); 77 | D3 = Pendulum_Servo_Add_Int_MO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u,yr(k)); 78 | D4 = Pendulum_Servo_Add_Int_MO_Obs_Proj(t+dt,Xj+D3*dt,u,yr(k)); 79 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 80 | X(:,k+1) = Xj; 81 | O1 = Pendulum_Min_Obser_Servo_Add_Int_Proj(t,Ethj,u,y); 82 | O2 = Pendulum_Min_Obser_Servo_Add_Int_Proj(t+dt/2,Ethj+O1*dt/2,u,y); 83 | O3 = Pendulum_Min_Obser_Servo_Add_Int_Proj(t+dt/2,Ethj+O2*dt/2,u,y); 84 | O4 = Pendulum_Min_Obser_Servo_Add_Int_Proj(t+dt,Ethj+O3*dt,u,y); 85 | Ethj = Ethj+(O1+2*O2+2*O3+O4)/6*dt; 86 | Eth(:,k+1) = Ethj; 87 | Xbh(:,k+1) = Ethj + Ko_Yx*Xj(1); 88 | Time(k+1) = t + dt; 89 | k = k + 1; 90 | t = t + dt; 91 | end 92 | 93 | %% Plots 94 | figure; 95 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'r'); 96 | title('Integrator Servo Design with Minimum Order Observer Initial Conddition "0,0,10,5"') 97 | xlabel('time(s)') 98 | ylabel('x(m)') 99 | legend('X','Yr','location','northeast') 100 | 101 | subplot(4,1,2);plot(Time,X(2,:),Time,Xbh(1,:),'g'); 102 | xlabel('time(s)') 103 | ylabel('xdot(m/s)') 104 | legend('X','Xbh','location','northeast') 105 | 106 | subplot(4,1,3);plot(Time,X(3,:),Time,Xbh(2,:),'g'); 107 | xlabel('time(s)') 108 | ylabel('theta(rad)') 109 | legend('X','Xbh','location','northeast') 110 | 111 | subplot(4,1,4);plot(Time,X(4,:),Time,Xbh(3,:),'g'); 112 | xlabel('time(s)') 113 | ylabel('thetadot(rad/s)') 114 | legend('X','Xbh','location','northeast') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward/Pendulum_Servo_FF_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_FF_Proj(t,X,u) 2 | 3 | global M_Cart m g l A B 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward/Project_Inverted_Pendulum_System_Servo_Feed_Forward.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Servo_Feed_Forward 2 | clc 3 | clear 4 | 5 | global M_Cart m g l A B 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | %% Designing Controller Gain, Ackerman Method 16 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 17 | r_M = rank(M); 18 | if r_M == min(size(M)) 19 | fprintf('The system is controllable and the rank of M is\n') 20 | disp(r_M) 21 | 22 | mu_d = [-3 -3 -2+2i -2-2i]; %% Desired Eigenvalues 23 | K_srv = acker(A_Linear,B_Linear,mu_d); 24 | 25 | fprintf('The Controller Gain "K" is\n') 26 | disp(K_srv) 27 | else 28 | disp('The System is Unctrollable') 29 | end 30 | 31 | 32 | %% Simulation 33 | T = 60; 34 | dt = 0.01; 35 | X0 = [0;0;0.349066;0.174533]; 36 | t = 0; 37 | X(:,1) = X0; 38 | Time(1) = t; 39 | k = 1; 40 | while t < T 41 | Xj = X(:,k); 42 | y = C_Yx*Xj; 43 | yr(k) = 0.5*sign(sin(0.2*t)); 44 | uff = (-yr(k))/(C_Yx*inv(A-B*K_srv)*B); 45 | u = -K_srv*Xj + uff; 46 | D1 = Pendulum_Servo_FF_Proj(t,Xj,u); 47 | D2 = Pendulum_Servo_FF_Proj(t+dt/2,Xj+D1*dt/2,u); 48 | D3 = Pendulum_Servo_FF_Proj(t+dt/2,Xj+D2*dt/2,u); 49 | D4 = Pendulum_Servo_FF_Proj(t+dt,Xj+D3*dt,u); 50 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 51 | X(:,k+1) = Xj; 52 | 53 | Time(k+1) = t + dt; 54 | k = k + 1; 55 | t = t + dt; 56 | end 57 | 58 | %% Plots 59 | figure; 60 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'g'); 61 | title('Feed Forward Servo Controller Design Initial Conddition "0,0,20,10"') 62 | xlabel('time(s)') 63 | ylabel('x(m)') 64 | legend('Y','Yr','location','northeast') 65 | 66 | subplot(4,1,2);plot(Time,X(2,:)); 67 | xlabel('time(s)') 68 | ylabel('xdot(m/s)') 69 | 70 | subplot(4,1,3);plot(Time,X(3,:)); 71 | xlabel('time(s)') 72 | ylabel('theta(rad)') 73 | 74 | subplot(4,1,4);plot(Time,X(4,:)); 75 | xlabel('time(s)') 76 | ylabel('thetadot(rad/s)') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward_Full_Order_Observer/Pendulum_Full_Obser_Servo_FF_Proj.m: -------------------------------------------------------------------------------- 1 | function DXh=Pendulum_Full_Obser_Servo_FF_Proj(t,Xh,u,y) 2 | global Ko_Yx A B C_Yx 3 | 4 | 5 | yh = C_Yx*Xh; 6 | DXh = A*Xh + B*u + Ko_Yx*(y-yh); -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward_Full_Order_Observer/Pendulum_Servo_FF_FO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_FF_FO_Obs_Proj(t,X,u) 2 | 3 | global A B m M_Cart g l 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | 8 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward_Full_Order_Observer/Project_Inverted_Pendulum_System_Servo_FF_FO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Servo_Feed_Forward_Full_Order_Observer 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C_Yx A B 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | 16 | %% Designing Controller Gain, Ackerman Method 17 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 18 | r_M = rank(M); 19 | if r_M == min(size(M)) 20 | fprintf('The system is controllable and the rank of M is\n') 21 | disp(r_M) 22 | 23 | mu_d = [-3 -3 -2+2i -2-2i]; %% Desired Eigenvalues 24 | K_srv = acker(A_Linear,B_Linear,mu_d); 25 | 26 | fprintf('The Controller Gain "K" is\n') 27 | disp(K_srv) 28 | else 29 | disp('The System is Unctrollable') 30 | end 31 | 32 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 33 | N_T_Yx = [C_Yx' A_Linear'*C_Yx' (A_Linear')^2*C_Yx' (A_Linear')^3*C_Yx']; %% N tranpose matrix when system observs X1=x 34 | r_N_T_Yx = rank(N_T_Yx); 35 | if r_N_T_Yx == min(size(N_T_Yx)) 36 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 37 | disp(r_N_T_Yx) 38 | 39 | muo_d = [-10 -10 -10 -10]; 40 | Ko_T_Yx = acker(A_Linear',C_Yx',muo_d); 41 | Ko_Yx = Ko_T_Yx'; 42 | 43 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 44 | disp(Ko_Yx) 45 | else 46 | disp('The system is unobservable when it observes (x)') 47 | end 48 | 49 | %% Simulation 50 | T = 60; 51 | dt = 0.001; 52 | X0 = [0;0;0.349066;0.174533]; 53 | Xh0 = [0.01;0.01;0.1;0.1]; 54 | t = 0; 55 | X(:,1) = X0; 56 | Xh(:,1) = Xh0; 57 | Time(1) = t; 58 | k = 1; 59 | while t < T 60 | Xj = X(:,k); 61 | Xhj = Xh(:,k); 62 | y = C_Yx*Xj; 63 | yr(k) = 0.5*sign(sin(0.2*t)); 64 | uff = (-yr(k))/(C_Yx*1/((A-B*K_srv))*B); 65 | u = -K_srv*Xj + uff; 66 | D1 = Pendulum_Servo_FF_FO_Obs_Proj(t,Xj,u); 67 | D2 = Pendulum_Servo_FF_FO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u); 68 | D3 = Pendulum_Servo_FF_FO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u); 69 | D4 = Pendulum_Servo_FF_FO_Obs_Proj(t+dt,Xj+D3*dt,u); 70 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 71 | X(:,k+1) = Xj; 72 | O1 = Pendulum_Full_Obser_Servo_FF_Proj(t,Xhj,u,y); 73 | O2 = Pendulum_Full_Obser_Servo_FF_Proj(t+dt/2,Xhj+O1*dt/2,u,y); 74 | O3 = Pendulum_Full_Obser_Servo_FF_Proj(t+dt/2,Xhj+O2*dt/2,u,y); 75 | O4 = Pendulum_Full_Obser_Servo_FF_Proj(t+dt,Xhj+O3*dt,u,y); 76 | Xhj = Xhj + (O1+2*O2+2*O3+O4)/6*dt; 77 | Xh(:,k+1) = Xhj; 78 | 79 | Time(k+1) = t+dt; 80 | k = k+1; 81 | t = t + dt; 82 | end 83 | 84 | %% Plots 85 | figure; 86 | subplot(4,1,1);plot(Time,X(1,:),Time,Xh(1,:),'g',Time(1:end-1),yr,'r'); 87 | title('Feed Forward Servo Design with Observer Initial Conddition "0,0,20,10"') 88 | xlabel('time(s)') 89 | ylabel('x(m)') 90 | legend('X','Xh','Yr','location','northeast') 91 | 92 | subplot(4,1,2);plot(Time,X(2,:),Time,Xh(2,:),'g'); 93 | xlabel('time(s)') 94 | ylabel('xdot(m/s)') 95 | legend('X','Xh','location','northeast') 96 | 97 | subplot(4,1,3);plot(Time,X(3,:),Time,Xh(3,:),'g'); 98 | xlabel('time(s)') 99 | ylabel('theta(rad)') 100 | legend('X','Xh','location','northeast') 101 | 102 | subplot(4,1,4);plot(Time,X(4,:),Time,Xh(4,:),'g'); 103 | xlabel('time(s)') 104 | ylabel('thetadot(rad/s)') 105 | legend('X','Xh','location','northeast') -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward_Minimum_Order_Observer/Pendulum_Min_Obser_Servo_FF_Proj.m: -------------------------------------------------------------------------------- 1 | function DEth=Pendulum_Min_Obser_Servo_FF_Proj(t,Eth,u,y) 2 | global Aaa Aab Aba Abb Ba Bb Ko_Yx 3 | 4 | DEth=Abb*Eth + Abb*Ko_Yx*y + Aba*y + Bb*u + Ko_Yx*(-Aaa*y - Aab*Eth - Aab*Ko_Yx*y - Ba*u); -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward_Minimum_Order_Observer/Pendulum_Servo_FF_MO_Obs_Proj.m: -------------------------------------------------------------------------------- 1 | function DX=Pendulum_Servo_FF_MO_Obs_Proj(t,X,u) 2 | 3 | global A B m M_Cart g l Aaa Aab Abb Aba Ba Bb 4 | 5 | A = [0 1 0 0;0 0 (-m*g*sin(X(3))*cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)) (m*l*X(4)*sin(X(3)))/(M_Cart+m*(sin(X(3)))^2);0 0 0 1;0 0 ((M_Cart+m)*g*sin(X(3)))/((M_Cart+m*(sin(X(3)))^2)*X(3)*l) (-m*l*X(4)*sin(X(3)*cos(X(3))))/((M_Cart+m*(sin(X(3)))^2)*l)]; 6 | B = [0;1/(M_Cart+m*(sin(X(3)))^2);0;(-cos(X(3)))/((M_Cart+m*(sin(X(3)))^2)*l)]; 7 | Aaa = A(1); 8 | Aab = A(1,2:4); 9 | Aba = A(2:4,1); 10 | Abb = [A(2,2:4);A(3,2:4);A(4,2:4)]; 11 | Ba = B(1); 12 | Bb = B(2:4,1); 13 | 14 | 15 | DX= A*X + B*u; -------------------------------------------------------------------------------- /Inverted_Pendulum_System_Servo_Feed_Forward_Minimum_Order_Observer/Project_Inverted_Pendulum_System_Servo_FF_MO_Obs.m: -------------------------------------------------------------------------------- 1 | %% Project#1_Advanced_Control_Inverted_Pendulum_System_Servo_Feed_Forward_Minimum_Order_Observer 2 | clc 3 | clear 4 | 5 | global M_Cart m g l C_Yx Ko_Yx A B 6 | %% System Parameters 7 | M_Cart = 2; %% Cart Mass 8 | m = 0.5; %% Pendulum Mass 9 | l = 1; %% Pendulum Beam Length 10 | g = 9.81; 11 | 12 | A_Linear = [0 1 0 0;0 0 (-m*g)/(M_Cart) 0;0 0 0 1; 0 0 ((M_Cart+m)*g)/(M_Cart*l) 0]; 13 | B_Linear = [0;1/M_Cart;0;(-1)/(M_Cart*l)]; 14 | C_Yx = [1 0 0 0]; 15 | Ah_Linear = [A_Linear zeros(4,1);-C_Yx 0]; 16 | Bh_Linear = [B_Linear;0]; 17 | Aaa_Linear = A_Linear(1); 18 | Aab_Linear = A_Linear(1,2:4); 19 | Aba_Linear = A_Linear(2:4,1); 20 | Abb_Linear = [A_Linear(2,2:4);A_Linear(3,2:4);A_Linear(4,2:4)]; 21 | Ba_Linear = B_Linear(1); 22 | Bb_Linear = B_Linear(2:4,1); 23 | 24 | %% Designing Controller Gain, Ackerman Method 25 | M = [B_Linear A_Linear*B_Linear A_Linear^2*B_Linear A_Linear^3*B_Linear]; 26 | r_M = rank(M); 27 | if r_M == min(size(M)) 28 | fprintf('The system is controllable and the rank of M is\n') 29 | disp(r_M) 30 | 31 | mu_d = [-3 -3 -2+2i -2-2i]; %% Desired Eigenvalues 32 | K_srv = acker(A_Linear,B_Linear,mu_d); 33 | 34 | fprintf('The Controller Gain "K" is\n') 35 | disp(K_srv) 36 | else 37 | disp('The System is Unctrollable') 38 | end 39 | 40 | %% Designing Observer Gain When System Observes X1=x, Ackerman Method 41 | N_T_Yx = [Aab_Linear' Abb_Linear'*Aab_Linear' (Abb_Linear')^2*Aab_Linear']; %% N tranpose matrix when system observs X1=x 42 | r_N_T_Yx = rank(N_T_Yx); 43 | if r_N_T_Yx == min(size(N_T_Yx)) 44 | fprintf('When system observes (x), it is Observable and the rank of N_Transpose is\n') 45 | disp(r_N_T_Yx) 46 | 47 | muo_d = [-10 -10 -10]; 48 | Ko_T_Yx = acker(Abb_Linear',Aab_Linear',muo_d); 49 | Ko_Yx = Ko_T_Yx'; 50 | 51 | fprintf('The Observer Gain "Ko" When System Observes (x) is\n') 52 | disp(Ko_Yx) 53 | else 54 | disp('The system is unobservable when it observes (x)') 55 | end 56 | 57 | %% Simulation 58 | T = 60; 59 | dt = 0.0001; 60 | X0 = [0;0;0.349066;0.174533]; 61 | Xbh0 = [0.01;0.1;0.1]; 62 | Eth0 = Xbh0 - Ko_Yx*X0(1); 63 | t = 0; 64 | X(:,1) = X0; 65 | Xbh(:,1) = Xbh0; 66 | Eth(:,1) = Eth0; 67 | Time(1) = t; 68 | k = 1; 69 | while t < T 70 | Xj = X(:,k); 71 | Ethj = Eth(:,k); 72 | Xbhj = Ethj + Ko_Yx*Xj(1); 73 | y = C_Yx*Xj(1:4); %%%% y=Xa=X1; 74 | yr(k) = 0.5*sign(sin(0.2*t)); 75 | uff = -yr(k)/(C_Yx*inv(A-B*K_srv)*B); 76 | u = -K_srv(1)*Xj(1) - K_srv(2:4)*Xbhj + uff; 77 | D1 = Pendulum_Servo_FF_MO_Obs_Proj(t,Xj,u); 78 | D2 = Pendulum_Servo_FF_MO_Obs_Proj(t+dt/2,Xj+D1*dt/2,u); 79 | D3 = Pendulum_Servo_FF_MO_Obs_Proj(t+dt/2,Xj+D2*dt/2,u); 80 | D4 = Pendulum_Servo_FF_MO_Obs_Proj(t+dt,Xj+D3*dt,u); 81 | Xj = Xj + (D1+2*D2+2*D3+D4)/6*dt; 82 | X(:,k+1) = Xj; 83 | O1 = Pendulum_Min_Obser_Servo_FF_Proj(t,Ethj,u,y); 84 | O2 = Pendulum_Min_Obser_Servo_FF_Proj(t+dt/2,Ethj+O1*dt/2,u,y); 85 | O3 = Pendulum_Min_Obser_Servo_FF_Proj(t+dt/2,Ethj+O2*dt/2,u,y); 86 | O4 = Pendulum_Min_Obser_Servo_FF_Proj(t+dt,Ethj+O3*dt,u,y); 87 | Ethj = Ethj+(O1+2*O2+2*O3+O4)/6*dt; 88 | Eth(:,k+1) = Ethj; 89 | Xbh(:,k+1) = Ethj + Ko_Yx*Xj(1); 90 | Time(k+1) = t + dt; 91 | k = k + 1; 92 | t = t + dt; 93 | end 94 | 95 | %% Plots 96 | figure; 97 | subplot(4,1,1);plot(Time,X(1,:),Time(1:end-1),yr,'r'); 98 | title('Feed Forward Servo Design with Minimum Order Observer Initial Conddition "0,0,20,10"') 99 | xlabel('time(s)') 100 | ylabel('x(m)') 101 | legend('X','Yr','location','northeast') 102 | 103 | subplot(4,1,2);plot(Time,X(2,:),Time,Xbh(1,:),'g'); 104 | xlabel('time(s)') 105 | ylabel('xdot(m/s)') 106 | legend('X','Xbh','location','northeast') 107 | 108 | subplot(4,1,3);plot(Time,X(3,:),Time,Xbh(2,:),'g'); 109 | xlabel('time(s)') 110 | ylabel('theta(rad)') 111 | legend('X','Xbh','location','northeast') 112 | 113 | subplot(4,1,4);plot(Time,X(4,:),Time,Xbh(3,:),'g'); 114 | xlabel('time(s)') 115 | ylabel('thetadot(rad/s)') 116 | legend('X','Xbh','location','northeast') -------------------------------------------------------------------------------- /LOS_Guidamce_Noneideal_Missile_Dynamics_Lead_Lag_Compensator/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#2 2 | ro = 1.225; %kg/m^3% 3 | S = 0.1; %m^2% 4 | g = 0; %m/s^2% 5 | aty = 0; %m/s^2% 6 | atz = 0; %m/s^2% 7 | Gz = 25; %Constant Guidance Coefficient Elevation Channel% 8 | Gy = 25; %Constant Guidance Coefficient Azimuth Channel% 9 | gain = 9.3; 10 | %% Transfer_Function_of_Lead-Lag_Compensator 11 | num_lead_comp = [2 1]; 12 | den_lean_comp = [2 1 5]; 13 | lead_comp = tf(num_lead_comp,den_lean_comp); 14 | 15 | num_controller = [0 1]; 16 | den_controller = [0.2 1]; 17 | controller = tf(num_controller,den_controller); 18 | 19 | open_loop_tf = lead_comp*controller; 20 | close_loop_tf = feedback(open_loop_tf,1); 21 | 22 | %rlocus(close_loop_tf) -------------------------------------------------------------------------------- /LOS_Guidamce_Noneideal_Missile_Dynamics_Lead_Lag_Compensator/Plots_Problme_1_Part_E.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_HW#2_Problem_1_Part_E 2 | 3 | %% Missile 4 | %% X_m-Time Diagram 5 | figure('Name','X_m-Time Diagram') 6 | plot(X_m) 7 | title('Missile X Position Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 8 | xlabel('Time (sec)') 9 | ylabel('X_m (m)') 10 | 11 | %% Y_m-Time Diagram 12 | figure('Name','Y_m-Time Diagram') 13 | plot(Y_m) 14 | title('Missile Y Position Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 15 | xlabel('Time (sec)') 16 | ylabel('Y_m (m)') 17 | 18 | %% Z_m-Time Diagram 19 | figure('Name','Z_m-Time Diagram') 20 | plot(Z_m) 21 | title('Missile Z Position Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 22 | xlabel('Time (sec)') 23 | ylabel('Z_m (m)') 24 | 25 | %% am_x-Time Diagram 26 | figure('Name','am_x-Time Diagram') 27 | plot(am_x) 28 | title('Missile X Guidance Acceleration Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 29 | xlabel('Time (sec)') 30 | ylabel('am_x (m/s^2)') 31 | 32 | %% am_y-Time Diagram 33 | figure('Name','am_y-Time Diagram') 34 | plot(am_y) 35 | title('Missile Y Guidance Acceleration Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 36 | xlabel('Time (sec)') 37 | ylabel('am_y (m/s^2)') 38 | 39 | %% am_z-Time Diagram 40 | figure('Name','am_z-Time Diagram') 41 | plot(am_z) 42 | title('Missile Z Guidance Acceleration Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 43 | xlabel('Time (sec)') 44 | ylabel('am_z (m/s^2)') 45 | 46 | %% Missile Flight Path Diagram 47 | figure('Name','Flight Path XY Plane Diagram') 48 | plot(X_m.Data,Y_m.Data) 49 | title('Missile Flight Path XY Plane(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 50 | xlabel('X (m)') 51 | ylabel('Y (m)') 52 | 53 | figure('Name','Flight Path XZ Plane Diagram') 54 | plot(X_m.Data,Z_m.Data) 55 | title('Missile Flight Path XZ Plane(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 56 | xlabel('X (m)') 57 | ylabel('Z (m)') 58 | 59 | figure('Name','Flight Path Diagram 3D') 60 | plot3(X_m.Data,Y_m.Data,Z_m.Data) 61 | title('Missile Flight Path 3D(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 62 | grid on 63 | xlabel('X (m)') 64 | ylabel('Y (m)') 65 | zlabel('Z (m)') 66 | 67 | %% Target 68 | %% X_t-Time Diagram 69 | figure('Name','X_t-Time Diagram') 70 | plot(X_t) 71 | title('Target X Position Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 72 | xlabel('Time (sec)') 73 | ylabel('X_t (m)') 74 | 75 | %% Y_t-Time Diagram 76 | figure('Name','Y_t-Time Diagram') 77 | plot(Y_t) 78 | title('Target Y Position Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 79 | xlabel('Time (sec)') 80 | ylabel('Y_t (m)') 81 | 82 | %% Z_t-Time Diagram 83 | figure('Name','Z_t-Time Diagram') 84 | plot(Z_t) 85 | title('Target Z Position Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 86 | xlabel('Time (sec)') 87 | ylabel('Z_t (m)') 88 | 89 | %% Target Flight Path Diagram 90 | figure('Name','Flight Path XY Plane Diagram') 91 | plot(X_t.Data,Y_t.Data) 92 | title('Target Flight Path XY Plane(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 93 | xlabel('X (m)') 94 | ylabel('Y (m)') 95 | 96 | figure('Name','Flight Path XZ Plane Diagram') 97 | plot(X_t.Data,Z_t.Data) 98 | title('Target Flight Path XZ Plane(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 99 | xlabel('X (m)') 100 | ylabel('Z (m)') 101 | 102 | figure('Name','Flight Path Diagram 3D') 103 | plot3(X_t.Data,Y_t.Data,Z_t.Data) 104 | title('Target Flight Path 3D(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 105 | grid on 106 | xlabel('X (m)') 107 | ylabel('Y (m)') 108 | zlabel('Z (m)') 109 | 110 | %% Missile_and_Target_Flight_Path 111 | figure('Name','Flight Path XY Plane Diagram') 112 | plot(X_m.Data,Y_m.Data) 113 | hold on 114 | plot(X_t.Data,Y_t.Data) 115 | title('Missile and Target Flight Path XY Plane(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 116 | legend('Missile','Target') 117 | xlabel('X (m)') 118 | ylabel('Y (m)') 119 | 120 | figure('Name','Flight Path XZ Plane Diagram') 121 | plot(X_m.Data,Z_m.Data) 122 | hold on 123 | plot(X_t.Data,Z_t.Data) 124 | title('Missile and Target Flight Path XZ Plane(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 125 | legend('Missile','Target') 126 | xlabel('X (m)') 127 | ylabel('Z (m)') 128 | 129 | figure('Name','Flight Path Diagram 3D') 130 | plot3(X_m.Data,Y_m.Data,Z_m.Data) 131 | hold on 132 | plot3(X_t.Data,Y_t.Data,Z_t.Data) 133 | title('Missile and Target Flight Path 3D(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 134 | legend('Missile','Target') 135 | grid on 136 | xlabel('X (m)') 137 | ylabel('Y (m)') 138 | zlabel('Z (m)') 139 | 140 | %% Miss_Distance 141 | figure('Name','MD-Time Diagram') 142 | plot(MD) 143 | title('Miss Distance Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 144 | xlabel('Time (sec)') 145 | ylabel('MD (m)') 146 | 147 | %% Control_Effort 148 | figure('Name','CE-Time Diagram') 149 | plot(CE) 150 | title('Control Effort Variation vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 151 | xlabel('Time (sec)') 152 | ylabel('CE (m/s)') 153 | 154 | %% Missile_Distance_To_Elevation_LOS 155 | figure('Name','d_epsilon-Time Diagram') 156 | plot(d_epsilon) 157 | title('Missile Distance To Elevation LOS vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 158 | xlabel('Time (sec)') 159 | ylabel('depsilon (m)') 160 | 161 | %% Missile_Distance_To_Azimuth_LOS 162 | figure('Name','d_sigma-Time Diagram') 163 | plot(d_sigma) 164 | title('Missile Distance To Azimuth LOS vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 165 | xlabel('Time (sec)') 166 | ylabel('dsigma (m)') 167 | 168 | %% Missile_Distance_To_Elevation_LOS vs Missile_Distance_To_Azimuth_LOS 169 | figure('Name','Missile Distance To Elevation LOS vs Missile Distance To Azimuth LOS Diagram') 170 | plot(d_sigma.Data,d_epsilon.Data) 171 | title('Missile Distance To Elevation LOS vs Missile Distance To Azimuth LOS(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 172 | xlabel('dsigma (m)') 173 | ylabel('depsilon (m)') 174 | 175 | %% Difference_of_Missile_Elevation_LOS_Angle_and_Target_Elevation_LOS_Angle 176 | figure('Name','delta_epsilon-Time Diagram') 177 | plot(delta_epsilon) 178 | title('Difference Of Missile Elevation LOS Angle & Target Elevation LOS Angle vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 179 | xlabel('Time (sec)') 180 | ylabel('delta epsilon (rad)') 181 | 182 | %% Difference_of_Missile_Azimuth_LOS_Angle_and_Target_Azimuth_LOS_Angle 183 | figure('Name','delta_sigma-Time Diagram') 184 | plot(delta_sigma) 185 | title('Difference Of Missile Azmiuth LOS Angle & Target Azimuth LOS Angle vs Time(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 186 | xlabel('Time (sec)') 187 | ylabel('delta sigma (rad)') 188 | 189 | %% Acceleration_Command_VS_Actual_Acceleration_Azimuth_Channel 190 | figure('Name','Acceleration_Command_VS_Actual_Acceleration_Azimuth_Channel') 191 | plot(ac_y) 192 | hold on 193 | plot(am_y) 194 | title('Acceleration Command vs Actual Acceleration Azimuth Channel(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 195 | legend('ac_y','am_y') 196 | xlabel('Time (s)') 197 | ylabel('Acceleration (m/s^2)') 198 | 199 | %% Acceleration_Command_VS_Actual_Acceleration_Elevation_Channel 200 | figure('Name','Acceleration_Command_VS_Actual_Acceleration_Elevation_Channel') 201 | plot(ac_z) 202 | hold on 203 | plot(am_z) 204 | title('Acceleration Command vs Actual Acceleration Elevation Channel(None Zero Dyanmic & Lead-Lag Guidance Coeff.)') 205 | legend('ac_z','am_z') 206 | xlabel('Time (s)') 207 | ylabel('Acceleration (m/s^2)') 208 | 209 | min(MD) -------------------------------------------------------------------------------- /LOS_Guidamce_Noneideal_Missile_Dynamics_Lead_Lag_Compensator/Problem_1_Part_E.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/LOS_Guidamce_Noneideal_Missile_Dynamics_Lead_Lag_Compensator/Problem_1_Part_E.slx -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#4 2 | 3 | Long0 = 35; %degree% 4 | Longf = 50; %degree% 5 | a = 6371000; %m% 6 | R0 = 6371000; %m% 7 | Rf = 6371000; %m% 8 | GM = 3.986004418*(10^14); %m^3/s^2% 9 | Phi = (Longf - Long0)*(pi/180);%rad% 10 | Gamma_min = atan(sin(Phi) - (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 11 | Gamma_max = atan(sin(Phi) + (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 12 | Gamma0 = 60*pi/180;%rad% 13 | V0 = 10;%m/s% 14 | errorV = 1; -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance/Plots_Problme_1_Part_B.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_HW#4_Problem_1_Part_B 2 | 3 | %% Missile 4 | %% X_m-Time Diagram 5 | figure('Name','X-Time Diagram') 6 | plot(t,X,'b') 7 | grid on 8 | title('Missile X Position Variation due to Time') 9 | xlabel('Time (sec)') 10 | ylabel('X (m)') 11 | 12 | %% Y-Time Diagram 13 | figure('Name','Y-Time Diagram') 14 | plot(t,Y,'b') 15 | grid on 16 | title('Missile Y Position Variation due to Time') 17 | xlabel('Time (sec)') 18 | ylabel('Y (m)') 19 | 20 | %% R-Time Diagram 21 | figure('Name','R-Time Diagram') 22 | plot(t,R,'b') 23 | grid on 24 | title('Missile Altitude Variation due to Time') 25 | xlabel('Time (sec)') 26 | ylabel('R (m)') 27 | 28 | %% Longtitude-Time Diagram 29 | figure('Name','Longtitude-Time Diagram') 30 | plot(t,Theta,'b') 31 | grid on 32 | title('Longtitude Variation due to Time') 33 | xlabel('Time (sec)') 34 | ylabel('Longtitude (deg)') 35 | 36 | %% Velocity-Time Diagram 37 | figure('Name','Velocity-Time Diagram') 38 | plot(t,V,'b') 39 | grid on 40 | title('Missile Velocity Variation due to Time') 41 | xlabel('Time (sec)') 42 | ylabel('V (m/s)') 43 | 44 | %% Total R-Acceleration-Time Diagram 45 | figure('Name','Total R-Acceleration-Time Diagram') 46 | plot(t,atotal_R,'b') 47 | grid on 48 | title('Total R-Acceleration due to Time') 49 | xlabel('Time (sec)') 50 | ylabel('atotal_R (m/s^2)') 51 | 52 | %% Total Theta-Acceleration-Time Diagram 53 | figure('Name','Total Theta-Acceleration-Time Diagram') 54 | plot(t,atotal_Theta,'b') 55 | grid on 56 | title('Total Theta-Acceleration due to Time') 57 | xlabel('Time (sec)') 58 | ylabel('atotal_Theta (m/s^2)') 59 | 60 | %% Required and Instantenous X-Velocity-Time Diagram 61 | figure('Name','Required and Instantenous X-Velocity-Time Diagram') 62 | plot(t,V_X,'b') 63 | grid on 64 | hold on 65 | plot(t,Vr_X,'g') 66 | grid on 67 | legend('Vx','Vrx') 68 | title('Required and Instantenous X-Velocity due to Time') 69 | xlabel('Time (sec)') 70 | ylabel('X-Vlocity (m/s)') 71 | axis([0 48.794 -inf inf]) 72 | %% Required and Instantenous Y-Velocity-Time Diagram 73 | figure('Name','Required and Instantenous Y-Velocity-Time Diagram') 74 | plot(t,V_X,'b') 75 | grid on 76 | hold on 77 | plot(t,Vr_X,'g') 78 | grid on 79 | legend('Vy','Vry') 80 | title('Required and Instantenous Y-Velocity due to Time') 81 | xlabel('Time (sec)') 82 | ylabel('Y-Vlocity (m/s)') 83 | axis([0 48.794 -inf inf]) -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance/Problem_1_Part_B.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Lambert_Ballistic_Guidance/Problem_1_Part_B.slx -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Monte_Carlo_Simulation/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#4 2 | 3 | Long0 = 35; %degree% 4 | Longf = 50; %degree% 5 | a = 6371000; %m% 6 | R0 = 6371000; %m% 7 | Rf = 6371000; %m% 8 | GM = 3.986004418*(10^14); %m^3/s^2% 9 | Phi = (Longf - Long0)*(pi/180);%rad% 10 | Gamma_min = atan(sin(Phi) - (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 11 | Gamma_max = atan(sin(Phi) + (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 12 | Gamma0 = 60*pi/180;%rad% 13 | V0 = 10;%m/s% 14 | errorV = 1; -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Monte_Carlo_Simulation/Problem_1_Part_C3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Lambert_Ballistic_Guidance_Monte_Carlo_Simulation/Problem_1_Part_C3.slx -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Monte_Carlo_Simulation/Velocity_Acceleration_Measurement_Uncertinty.m: -------------------------------------------------------------------------------- 1 | %% Velocity_Measurement_Uncertinty_Guidance_and_Navigation_HW#4_Problem1_PartC1 2 | clc 3 | clear all 4 | mu1 = 0; 5 | sigma1 = 0.1; 6 | Vu = normrnd(mu1,sigma1); 7 | mu2 = 0; 8 | sigma2 = 0.001; 9 | au = normrnd(mu2,sigma2); 10 | i = 1; 11 | Xreal = 4095873; 12 | for i = 1:1:100 13 | 14 | sim('Problem_1_Part_C3') 15 | mu1 = 0; 16 | sigma1 = 0.1; 17 | Vu = normrnd(mu1,sigma1); 18 | mu2 = 0; 19 | sigma2 = 0.001; 20 | au = normrnd(mu2,sigma2); 21 | Range(i) = X(end); 22 | 23 | end 24 | 25 | %% Range Error Standard Deviation and Plot 26 | ERange = Xreal - Range; 27 | i = 1:1:100; 28 | figure('Name','Range Error_(Vu_au) Diagram') 29 | plot(i,ERange) 30 | grid on 31 | title('Range Error vs Velocity and Acceleration Uncertainty') 32 | xlabel('Number of Iterations') 33 | ylabel('Range (m)') 34 | 35 | Mean = mean(ERange); 36 | Standard_Deviation = std(ERange); 37 | %% Display 38 | fprintf('The Range Error Average is (m)\n') 39 | disp(Mean) 40 | 41 | fprintf('The Range Error Standard Deviation is (m)\n') 42 | disp(Standard_Deviation) -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Secant_Method/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#4 2 | 3 | Long0 = 25; %degree% 4 | Longf = 50; %degree% 5 | a = 6371000; %m% 6 | R0 = 6371000; %m% 7 | Rf = 6371000; %m% 8 | GM = 3.986004418*(10^14); %m^3/s^2% 9 | Phi = (Longf - Long0)*(pi/180);%rad% 10 | Gamma_min = atan(sin(Phi) - (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 11 | Gamma_max = atan(sin(Phi) + (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 12 | Gamma0 = 0.3446;%rad% 13 | V0 = 5193;%m/s% -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Secant_Method/Plots_Problme_1_Part_A.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_HW#4_Problem_1_Part_A 2 | 3 | %% Missile 4 | %% X_m-Time Diagram 5 | figure('Name','X-Time Diagram') 6 | plot(X) 7 | grid on 8 | title('Missile X Position Variation due to Time') 9 | xlabel('Time (sec)') 10 | ylabel('X (m)') 11 | 12 | %% Y-Time Diagram 13 | figure('Name','Y-Time Diagram') 14 | plot(Y) 15 | grid on 16 | title('Missile Y Position Variation due to Time') 17 | xlabel('Time (sec)') 18 | ylabel('Y (m)') 19 | 20 | %% R-Time Diagram 21 | figure('Name','R-Time Diagram') 22 | plot(R) 23 | grid on 24 | title('Missile Altitude Variation due to Time') 25 | xlabel('Time (sec)') 26 | ylabel('R (m)') 27 | 28 | %% Longtitude-Time Diagram 29 | figure('Name','Longtitude-Time Diagram') 30 | plot(Theta) 31 | grid on 32 | title('Longtitude Variation due to Time') 33 | xlabel('Time (sec)') 34 | ylabel('Longtitude (deg)') 35 | 36 | %% Gamma-Time Diagram 37 | figure('Name','Gamma-Time Diagram') 38 | plot(Gamma) 39 | grid on 40 | title('Missile Flight Path Angle Variation due to Time') 41 | xlabel('Time (sec)') 42 | ylabel('Gamma (deg)') 43 | 44 | %% Velocity-Time Diagram 45 | figure('Name','Velocity-Time Diagram') 46 | plot(V) 47 | grid on 48 | title('Missile Velocity Variation due to Time') 49 | xlabel('Time (sec)') 50 | ylabel('V (m/s)') -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Secant_Method/Problem_1_Part_A.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Lambert_Ballistic_Guidance_Secant_Method/Problem_1_Part_A.slx -------------------------------------------------------------------------------- /Lambert_Ballistic_Guidance_Secant_Method/Secant_Method.m: -------------------------------------------------------------------------------- 1 | %% Secant_Method_Guidance_and_Navigation_HW#4 2 | 3 | clear all 4 | clc 5 | 6 | Long0 = 25; %degree% 7 | Theta = Long0*pi/180; 8 | Longf = 50; %degree% 9 | tfd = 600; %s% 10 | a = 6371000; %m% 11 | R0 = 6371000; %m% 12 | Rf = 6371000; %m% 13 | GM = 3.986004418*(10^14); %m^3/s^2% 14 | Phi = (Longf - Long0)*(pi/180);%rad% 15 | error = 0.00001; 16 | 17 | Gamma_min = atan(sin(Phi) - (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 18 | Gamma_max = atan(sin(Phi) + (sqrt((2*R0/Rf)*(1-cos(Phi))))/(1-cos(Phi)));%rad% 19 | Gamma0 = ((Gamma_max + Gamma_min)/2);%rad% 20 | V0x = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(Gamma0))*((R0*cos(Gamma0))/(a) - cos(Phi+Gamma0))) )*sin(Gamma0)*cos(Theta);%m/s% 21 | V0y = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(Gamma0))*((R0*cos(Gamma0))/(a) - cos(Phi+Gamma0))) )*sin(Gamma0)*sin(Theta); 22 | V0 = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(Gamma0))*((R0*cos(Gamma0))/(a) - cos(Phi+Gamma0))) ); 23 | landa0 = (R0*V0^2)/GM; 24 | tf0 = ( (R0)/(V0*cos(Gamma0)) )*( ( tan(Gamma0)*(1-cos(Phi)) + (1-landa0)*sin(Phi) )/( (2-landa0)*( (1-cos(Phi))/(landa0*cos(Gamma0)^2) + (cos(Gamma0 + Phi))/(cos(Gamma0)) ) ) + ( (2*cos(Gamma0))/(landa0*((2/landa0) - 1)^1.5) )*atan((sqrt((2/landa0) - 1)/((cos(Gamma0)*cot(Phi/2)) - (sin(Gamma0)))) )); 25 | 26 | Gamma00 = 0; 27 | tf00 = 0; 28 | V00x = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(Gamma00))*((R0*cos(Gamma00))/(a) - cos(Phi+Gamma00))) )*sin(Gamma00)*cos(Theta);%m/s% 29 | V00y = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(Gamma00))*((R0*cos(Gamma00))/(a) - cos(Phi+Gamma00))) )*sin(Gamma00)*sin(Theta);%m/s% 30 | V00 = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(Gamma00))*((R0*cos(Gamma00))/(a) - cos(Phi+Gamma00))) ); 31 | 32 | 33 | Gamma = []; 34 | tf = []; 35 | Vr = []; 36 | Vrx = []; 37 | Vry = []; 38 | Gamma(1) = Gamma00; 39 | tf(1) = tf00; 40 | Vr(1) = V00; 41 | Vrx(1) = V00x; 42 | Vry(1) = V00y; 43 | Gamma(2) = Gamma0; 44 | tf(2) = tf0; 45 | Vr(2) = V0; 46 | Vrx(1) = V0x; 47 | Vry(1) = V0y; 48 | i = 2; 49 | 50 | %% Secant Algorithm 51 | while abs(tfd - tf(i)) >= error 52 | 53 | Gamma(i+1) = Gamma(i) + ( (Gamma(i) - Gamma(i-1))*(tfd - tf(i)) )/(tf(i) - tf(i-1)); 54 | 55 | gamma = Gamma(i+1); 56 | Vrx(i+1) = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(gamma))*((R0*cos(gamma))/(a) - cos(Phi+gamma))) )*sin(gamma)*cos(Theta); 57 | Vry(i+1) = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(gamma))*((R0*cos(gamma))/(a) - cos(Phi+gamma))) )*sin(gamma)*sin(Theta); 58 | Vr(i+1) = sqrt( (GM*( 1-cos(Phi)))/((R0*cos(gamma))*((R0*cos(gamma))/(a) - cos(Phi+gamma))) ); 59 | vr = Vr(i+1); 60 | landa = (R0*vr^2)/GM; 61 | tf(i+1) = ( (R0)/(vr*cos(gamma)) )*( ( tan(gamma)*(1-cos(Phi)) + (1-landa)*sin(Phi) )/( (2-landa)*( (1-cos(Phi))/(landa*cos(gamma)^2) + (cos(gamma + Phi))/(cos(gamma)) ) ) + ( (2*cos(gamma))/(landa*((2/landa) - 1)^1.5) )*atan2((sqrt((2/landa) - 1)),((cos(gamma)*cot(Phi/2)) - (sin(gamma)))) ); 62 | 63 | 64 | i = i+1; 65 | 66 | end 67 | 68 | %% Display 69 | fprintf('The Initial Flight Path Angle Must be (deg)\n') 70 | disp(Gamma(end)*180/pi) 71 | 72 | fprintf('The Initial X-Velocity Must be (m/s)\n') 73 | disp(Vrx(end)) 74 | 75 | fprintf('The Initial Y-Velocity Must be (m/s)\n') 76 | disp(Vry(end)) 77 | 78 | fprintf('The Initial Velocity Must be (m/s)\n') 79 | disp(Vr(end)) 80 | 81 | fprintf('Flight Time\n') 82 | disp(tf(end)) -------------------------------------------------------------------------------- /Lead_Angle_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#2 2 | ro = 1.225; %kg/m^3% 3 | S = 0.1; %m^2% 4 | g = 9.81; %m/s^2% 5 | aty = 0*g; %m/s^2% 6 | atz = 0*g; %m/s^2% 7 | Gz = 2; %Constant Guidance Coefficient Elevation Channel% 8 | Gy = 2; %Constant Guidance Coefficient Azimuth Channel% 9 | A = 1/2; 10 | B = 1/20; 11 | gain = 9.07; 12 | %% Transfer_Function_of_Lead-Lag_Compensator 13 | num_lead_comp = [A 1]; 14 | den_lean_comp = [B 1]; 15 | lead_comp = tf(num_lead_comp,den_lean_comp); 16 | 17 | num_controller = [0 1]; 18 | den_controller = [0.2 1]; 19 | controller = tf(num_controller,den_controller); 20 | 21 | open_loop_tf = lead_comp*controller; 22 | close_loop_tf = feedback(open_loop_tf,1); 23 | 24 | %rlocus(open_loop_tf); -------------------------------------------------------------------------------- /Lead_Angle_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration/Problem_3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Lead_Angle_Guidance__Noneideal_Missile_Dynamics_Lead_Lag_Compensator_Gravity_Compensation_Saturated_Acceleration/Problem_3.slx -------------------------------------------------------------------------------- /Persuit_Guidance_Noneideal_Missile_Dynamics/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#3 2 | 3 | ro = 1.225; %kg/m^3% 4 | S = 0.1; %m^2% 5 | g = 0; %m/s^2% 6 | aty = 0; %m/s^2%d 7 | atz = 0; %m/s^2% 8 | G_p = 7.5; 9 | T_1 = 0; 10 | T_2 = 0; 11 | T = 0.2; 12 | R_min = 10; 13 | 14 | Psi_m0 = 10*pi/180; 15 | Theta_m0 = 24*pi/180; 16 | V_m0B = [1000;0;0]; 17 | R_Psi = [cos(Psi_m0) sin(Psi_m0) 0;-sin(Psi_m0) cos(Psi_m0) 0;0 0 1]; 18 | R_Theta = [cos(Theta_m0) 0 -sin(Theta_m0);0 1 0;sin(Theta_m0) 0 cos(Theta_m0)]; 19 | R_BI = R_Theta*R_Psi; 20 | R_IB = R_BI'; 21 | V_m0I = V_m0B; -------------------------------------------------------------------------------- /Persuit_Guidance_Noneideal_Missile_Dynamics/Plots_Problme_1_Part_A.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_HW#3_Problem_1_Part_A 2 | 3 | %% Missile 4 | %% X_m-Time Diagram 5 | figure('Name','X_m-Time Diagram') 6 | plot(X_m) 7 | grid on 8 | title('Missile X Position Variation vs Time') 9 | xlabel('Time (sec)') 10 | ylabel('X_m (m)') 11 | 12 | %% Y_m-Time Diagram 13 | figure('Name','Y_m-Time Diagram') 14 | plot(Y_m) 15 | grid on 16 | title('Missile Y Position Variation vs Time') 17 | xlabel('Time (sec)') 18 | ylabel('Y_m (m)') 19 | 20 | %% Z_m-Time Diagram 21 | figure('Name','Z_m-Time Diagram') 22 | plot(Z_m) 23 | grid on 24 | title('Missile Z Position Variation vs Time') 25 | xlabel('Time (sec)') 26 | ylabel('Z_m (m)') 27 | 28 | %% am_x-Time Diagram 29 | figure('Name','am_x-Time Diagram') 30 | plot(am_x) 31 | grid on 32 | title('Missile X Guidance Acceleration Variation vs Time') 33 | xlabel('Time (sec)') 34 | ylabel('am_x (m/s^2)') 35 | 36 | %% am_y-Time Diagram 37 | figure('Name','am_y-Time Diagram') 38 | plot(am_y) 39 | grid on 40 | title('Missile Y Guidance Acceleration Variation vs Time') 41 | xlabel('Time (sec)') 42 | ylabel('am_y (m/s^2)') 43 | 44 | %% am_z-Time Diagram 45 | figure('Name','am_z-Time Diagram') 46 | plot(am_z) 47 | grid on 48 | title('Missile Z Guidance Acceleration Variation vs Time') 49 | xlabel('Time (sec)') 50 | ylabel('am_z (m/s^2)') 51 | 52 | %% Missile Flight Path Diagram 53 | figure('Name','Flight Path XY Plane Diagram') 54 | plot(X_m.Data,Y_m.Data) 55 | grid on 56 | title('Missile Flight Path XY Plane') 57 | xlabel('X (m)') 58 | ylabel('Y (m)') 59 | 60 | figure('Name','Flight Path XZ Plane Diagram') 61 | plot(X_m.Data,Z_m.Data) 62 | grid on 63 | title('Missile Flight Path XZ Plane') 64 | xlabel('X (m)') 65 | ylabel('Z (m)') 66 | 67 | figure('Name','Flight Path Diagram 3D') 68 | plot3(X_m.Data,Y_m.Data,Z_m.Data) 69 | title('Missile Flight Path 3D') 70 | grid on 71 | xlabel('X (m)') 72 | ylabel('Y (m)') 73 | zlabel('Z (m)') 74 | 75 | %% Target 76 | %% X_t-Time Diagram 77 | figure('Name','X_t-Time Diagram') 78 | plot(X_t) 79 | grid on 80 | title('Target X Position Variation vs Time') 81 | xlabel('Time (sec)') 82 | ylabel('X_t (m)') 83 | 84 | %% Y_t-Time Diagram 85 | figure('Name','Y_t-Time Diagram') 86 | plot(Y_t) 87 | grid on 88 | title('Target Y Position Variation vs Time') 89 | xlabel('Time (sec)') 90 | ylabel('Y_t (m)') 91 | 92 | %% Z_t-Time Diagram 93 | figure('Name','Z_t-Time Diagram') 94 | plot(Z_t) 95 | grid on 96 | title('Target Z Position Variation vs Time') 97 | xlabel('Time (sec)') 98 | ylabel('Z_t (m)') 99 | 100 | %% Target Flight Path Diagram 101 | figure('Name','Flight Path XY Plane Diagram') 102 | plot(X_t.Data,Y_t.Data) 103 | grid on 104 | title('Target Flight Path XY Plane') 105 | xlabel('X (m)') 106 | ylabel('Y (m)') 107 | 108 | figure('Name','Flight Path XZ Plane Diagram') 109 | plot(X_t.Data,Z_t.Data) 110 | grid on 111 | title('Target Flight Path XZ Plane') 112 | xlabel('X (m)') 113 | ylabel('Z (m)') 114 | 115 | figure('Name','Flight Path Diagram 3D') 116 | plot3(X_t.Data,Y_t.Data,Z_t.Data) 117 | title('Target Flight Path 3D') 118 | grid on 119 | xlabel('X (m)') 120 | ylabel('Y (m)') 121 | zlabel('Z (m)') 122 | 123 | %% Missile_and_Target_Flight_Path 124 | figure('Name','Flight Path XY Plane Diagram') 125 | plot(X_m.Data,Y_m.Data) 126 | grid on 127 | hold on 128 | plot(X_t.Data,Y_t.Data) 129 | grid on 130 | title('Missile and Target Flight Path XY Plane') 131 | legend('Missile','Target') 132 | xlabel('X (m)') 133 | ylabel('Y (m)') 134 | 135 | figure('Name','Flight Path XZ Plane Diagram') 136 | plot(X_m.Data,Z_m.Data) 137 | grid on 138 | hold on 139 | plot(X_t.Data,Z_t.Data) 140 | grid on 141 | title('Missile and Target Flight Path XZ Plane') 142 | legend('Missile','Target') 143 | xlabel('X (m)') 144 | ylabel('Z (m)') 145 | 146 | figure('Name','Flight Path Diagram 3D') 147 | plot3(X_m.Data,Y_m.Data,Z_m.Data) 148 | grid on 149 | hold on 150 | plot3(X_t.Data,Y_t.Data,Z_t.Data) 151 | grid on 152 | title('Missile and Target Flight Path 3D') 153 | legend('Missile','Target') 154 | xlabel('X (m)') 155 | ylabel('Y (m)') 156 | zlabel('Z (m)') 157 | 158 | %% Miss_Distance 159 | figure('Name','MD-Time Diagram') 160 | plot(MD) 161 | grid on 162 | title('Miss Distance Variation vs Time') 163 | xlabel('Time (sec)') 164 | ylabel('MD (m)') 165 | 166 | %% Control_Effort 167 | figure('Name','CE-Time Diagram') 168 | plot(CE) 169 | grid on 170 | title('Control Effort Variation vs Time') 171 | xlabel('Time (sec)') 172 | ylabel('CE (m/s)') 173 | 174 | %% Acceleration_Command_VS_Actual_Acceleration_Azimuth_Channel 175 | figure('Name','Acceleration_Command_VS_Actual_Acceleration_Azimuth_Channel') 176 | plot(ac_y) 177 | grid on 178 | hold on 179 | plot(am_y) 180 | grid on 181 | title('Acceleration Command vs Actual Acceleration Azimuth Channel') 182 | legend('ac_y','am_y') 183 | xlabel('Time (s)') 184 | ylabel('Acceleration (m/s^2)') 185 | 186 | %% Acceleration_Command_VS_Actual_Acceleration_Elevation_Channel 187 | figure('Name','Acceleration_Command_VS_Actual_Acceleration_Elevation_Channel') 188 | plot(ac_z) 189 | grid on 190 | hold on 191 | plot(am_z) 192 | grid on 193 | title('Acceleration Command vs Actual Acceleration Elevation Channel') 194 | legend('ac_z','am_z') 195 | xlabel('Time (s)') 196 | ylabel('Acceleration (m/s^2)') 197 | 198 | %% Look_Angle 199 | figure('Name','L-Time Diagram') 200 | plot(L) 201 | grid on 202 | title('Look Angle Variation vs Time') 203 | xlabel('Time (sec)') 204 | ylabel('L (deg)') -------------------------------------------------------------------------------- /Persuit_Guidance_Noneideal_Missile_Dynamics/Problem_1_Part_A.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Persuit_Guidance_Noneideal_Missile_Dynamics/Problem_1_Part_A.slx -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Guidance-Navigation-and-Control 2 | Simulation and Implementation of some Guidance, Navigation, and Control Laws on UAV, Quadrotor, and Missile 3 | -------------------------------------------------------------------------------- /Sliding_Mode_Control_for_a_Quadotor/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Nonelinear_Control_Project 2 | 3 | %% Quadrotor Parameters 4 | m = 1.1; % mass 5 | l = 0.21; % m 6 | Ix = 0.022; % Ns^2/rad 7 | Iy = 0.022; % Ns^2/rad 8 | Iz = 0.042; % Ns^2/rad 9 | Jr = 0; % Ns^2/rad 10 | k1 = 0.001; % Ns/m 11 | k2 = 0.001; % Ns/M 12 | k3 = 0.001; % Ns/M 13 | k4 = 0.0012; % Ns/M 14 | k5 = 0.0012; % Ns/M 15 | k6 = 0.0012; % Ns/M 16 | g = 9.81; % m/s^2 17 | b = 5; % Ns^2 18 | k = 2; % N/ms^2 19 | omega = 2000; % rad/s 20 | %% Controller Parameters 21 | 22 | cz = 1; 23 | cpsi = 1; 24 | e1 = 30; 25 | e2 = 30; 26 | eta1 = 2; 27 | eta2 = 2; 28 | alpha1 = 10; 29 | alpha2 = 10; 30 | beta1 = 1; 31 | beta2 = 1; 32 | mperim1 = 7; 33 | mperim2 = 7; 34 | nperim1 = 9; 35 | nperim2 = 9; 36 | m1 = 1; 37 | m2 = 1; 38 | n1 = 3; 39 | n2 = 3; 40 | c3 = 1; 41 | c7 = 1; 42 | c4 = 6; 43 | c8 = 6; 44 | e3 = 10; 45 | e4 = 10; 46 | eta3 = 1; 47 | eta4 = 1; 48 | alpha3 = 10; 49 | alpha4 = 10; 50 | beta3 = 1; 51 | beta4 = 1; 52 | mperim3 = 7; 53 | mperim4 = 7; 54 | nperim3 = 9; 55 | nperim4 = 9; 56 | m3 = 1; 57 | m4 = 1; 58 | n3 = 3; 59 | n4 = 3; 60 | %% Desired Values 61 | x_desired = 0.6; 62 | x_dot_desired = 0; 63 | x_doubledot_desired = 0; 64 | x_tiersdot_desired = 0; 65 | y_desired = 0.6; 66 | y_dot_desired = 0; 67 | y_doubledot_desired = 0; 68 | y_tiersdot_desired = 0; 69 | z_desired = 0.6; 70 | z_dot_desired = 0; 71 | z_doubledot_desired = 0; 72 | z_tiersdot_desired = 0; 73 | Phi_desired = 0; 74 | Phi_dot_desired = 0; 75 | Phi_doubledot_desired = 0; 76 | Teta_desired = 0; 77 | Teta_dot_desired = 0; 78 | Teta_doubledot_desired = 0; 79 | Psi_desired = 0.5; 80 | Psi_dot_desired = 0; 81 | Psi_doubledot_desired = 0; 82 | Psi_tiersdot_desired = 0; 83 | -------------------------------------------------------------------------------- /Sliding_Mode_Control_for_a_Quadotor/Nonelinear_Control_Project.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/Sliding_Mode_Control_for_a_Quadotor/Nonelinear_Control_Project.slx -------------------------------------------------------------------------------- /Sliding_Mode_Control_for_a_Quadotor/Plots.m: -------------------------------------------------------------------------------- 1 | %% Nonelinear_Control_Project_Plots 2 | 3 | %% X-Time Diagram 4 | 5 | figure('Name','X-Time Diagram') 6 | plot(x) 7 | title('XInertia variation due to Time') 8 | xlabel('Time (sec)') 9 | ylabel('X (m)') 10 | 11 | %% Y-Time Diagram 12 | 13 | figure('Name','Y-Time Diagram') 14 | plot(y) 15 | title('YInertia variation due to Time') 16 | xlabel('Time (sec)') 17 | ylabel('Y (m)') 18 | 19 | %% Z-Time Diagram 20 | 21 | figure('Name','Z-Time Diagram') 22 | plot(z) 23 | title('ZInertia variation due to Time') 24 | xlabel('Time (sec)') 25 | ylabel('z (m)') 26 | 27 | %% U-Time Diagram 28 | 29 | figure('Name','U-Time Diagram') 30 | plot(U) 31 | title('Uvelocity variation due to Time') 32 | xlabel('Time (sec)') 33 | ylabel('U (m/s)') 34 | 35 | %% V-Time Diagram 36 | 37 | figure('Name','V-Time Diagram') 38 | plot(V) 39 | title('Vvelocity variation due to Time') 40 | xlabel('Time (sec)') 41 | ylabel('V (m/s)') 42 | 43 | %% W-Time Diagram 44 | 45 | figure('Name','W-Time Diagram') 46 | plot(W) 47 | title('Wvelocity variation due to Time') 48 | xlabel('Time (sec)') 49 | ylabel('W (m/s)') 50 | 51 | %% Roll velocity-Time Diagram 52 | 53 | figure('Name','Roll velocity-Time Diagram') 54 | plot(P) 55 | title('Roll velocity variation due to Time') 56 | xlabel('Time (sec)') 57 | ylabel('P (rad/s)') 58 | 59 | %% Pitch velocity-Time Diagram 60 | 61 | figure('Name','Pitch velocity-Time Diagram') 62 | plot(Q) 63 | title('Pitch velocity variation due to Time') 64 | xlabel('Time (sec)') 65 | ylabel('Q (rad/s)') 66 | 67 | %% Yaw velocity-Time Diagram 68 | 69 | figure('Name','Yaw velocity-Time Diagram') 70 | plot(R) 71 | title('Yaw velocity variation due to Time') 72 | xlabel('Time (sec)') 73 | ylabel('R (rad/s)') 74 | 75 | %% Phi-Time Diagram 76 | 77 | figure('Name','Phi-Time Diagram') 78 | plot(Phi) 79 | title('Phi variation due to Time') 80 | xlabel('Time (sec)') 81 | ylabel('Phi (rad)') 82 | 83 | %% Theta-Time Diagram 84 | 85 | figure('Name','Theta-Time Diagram') 86 | plot(Teta) 87 | title('Theta variation due to Time') 88 | xlabel('Time (sec)') 89 | ylabel('Theta (rad)') 90 | 91 | %% Psi-Time Diagram 92 | 93 | figure('Name','Psi-Time Diagram') 94 | plot(Psi) 95 | title('Psi variation due to Time') 96 | xlabel('Time (sec)') 97 | ylabel('Psi (rad)') 98 | 99 | %% U1-Time Diagram 100 | 101 | figure('Name','U1-Time Diagram') 102 | plot(u1) 103 | title('U1 variation due to Time') 104 | xlabel('Time (sec)') 105 | ylabel('U1 (N)') 106 | 107 | %% U2-Time Diagram 108 | 109 | figure('Name','U2-Time Diagram') 110 | plot(u2) 111 | title('U2 variation due to Time') 112 | xlabel('Time (sec)') 113 | ylabel('U2 (N)') 114 | 115 | %% U3-Time Diagram 116 | 117 | figure('Name','U3-Time Diagram') 118 | plot(u3) 119 | title('U3 variation due to Time') 120 | xlabel('Time (sec)') 121 | ylabel('U3 (N)') 122 | %% U4-Time Diagram 123 | 124 | figure('Name','U4-Time Diagram') 125 | plot(u4) 126 | title('U4 variation due to Time') 127 | xlabel('Time (sec)') 128 | ylabel('U4 (N)') 129 | %% dU1-Time Diagram 130 | 131 | figure('Name','dU1-Time Diagram') 132 | plot(u1dot) 133 | title('dU1 variation due to Time') 134 | xlabel('Time (sec)') 135 | ylabel('dU1 (N/s)') 136 | 137 | %% dU2-Time Diagram 138 | 139 | figure('Name','dU2-Time Diagram') 140 | plot(u2dot) 141 | title('dU2 variation due to Time') 142 | xlabel('Time (sec)') 143 | ylabel('dU2 (N/s)') 144 | 145 | %% dU3-Time Diagram 146 | 147 | figure('Name','dU3-Time Diagram') 148 | plot(u3dot) 149 | title('dU3 variation due to Time') 150 | xlabel('Time (sec)') 151 | ylabel('dU3 (N/s)') 152 | %% dU4-Time Diagram 153 | 154 | figure('Name','dU4-Time Diagram') 155 | plot(u4dot) 156 | title('dU4 variation due to Time') 157 | xlabel('Time (sec)') 158 | ylabel('dU4 (N/s)') 159 | %% S1-Time Diagram 160 | 161 | figure('Name','S1-Time Diagram') 162 | plot(s1) 163 | title('S1 variation due to Time') 164 | xlabel('Time (sec)') 165 | ylabel('S1') 166 | %% S3-Time Diagram 167 | 168 | figure('Name','S3-Time Diagram') 169 | plot(s3) 170 | title('S3 variation due to Time') 171 | xlabel('Time (sec)') 172 | ylabel('S3') 173 | %% Flight Path Diagram 174 | 175 | figure('Name','Flight Path Diagram') 176 | plot3(x.Data,y.Data,z.Data) 177 | title('Flight Path') 178 | xlabel('X (m)') 179 | ylabel('Y (m)') 180 | zlabel('Z (m)') 181 | -------------------------------------------------------------------------------- /UAV_Target_Following_and_State_Flow_Implementation/Initial_Data.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_HW#5 2 | 3 | %% UAV 4 | X0 = 500; %m 5 | Y0 = 0; %m 6 | V = 30; %m/s 7 | Psi0 = 0; %deg 8 | K = 5; 9 | desired_distance = 5; -------------------------------------------------------------------------------- /UAV_Target_Following_and_State_Flow_Implementation/Plots_Problme_1_Part_A.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_HW#5_Problem_1_Part_A 2 | 3 | %% X_UAV-Time Diagram 4 | figure('Name','X_UAV-Time Diagram') 5 | plot(X_UAV) 6 | grid on 7 | title('UAV X Position Variation due to Time') 8 | xlabel('Time (sec)') 9 | ylabel('X (m)') 10 | 11 | %% Y_UAV-Time Diagram 12 | figure('Name','Y_UAV-Time Diagram') 13 | plot(Y_UAV) 14 | grid on 15 | title('UAV Y Position Variation due to Time') 16 | xlabel('Time (sec)') 17 | ylabel('Y (m)') 18 | 19 | %% UAV Flight Path Diagram 20 | figure('Name','Flight Path XY Plane Diagram') 21 | plot(X_UAV.Data,Y_UAV.Data) 22 | grid on 23 | title('UAV Flight Path XY Plane') 24 | xlabel('X (m)') 25 | ylabel('Y (m)') 26 | 27 | %% Target Movement Path 28 | pgon = nsidedpoly(6,'Center',[750 433],'SideLength',500); 29 | figure('Name','Target Movement Path') 30 | plot(pgon) 31 | axis equal 32 | title('Target Movement Path Variation due to Time') 33 | xlabel('X (m)') 34 | ylabel('Y (m)') 35 | 36 | %% Xdot_UAV-Time Diagram 37 | figure('Name','Xdot_UAV-Time Diagram') 38 | plot(Xdot_UAV) 39 | grid on 40 | title('UAV X Velocity Variation due to Time') 41 | xlabel('Time (sec)') 42 | ylabel('Xdot (m/s)') 43 | 44 | %% Ydot_UAV-Time Diagram 45 | figure('Name','Ydot_UAV-Time Diagram') 46 | plot(Ydot_UAV) 47 | grid on 48 | title('UAV Y Velocity Variation due to Time') 49 | xlabel('Time (sec)') 50 | ylabel('Ydot (m/s)') 51 | 52 | %% Velocity-Time Diagram 53 | figure('Name','Velocity-Time Diagram') 54 | plot(V_UAV) 55 | grid on 56 | title('UAV Velocity Variation due to Time') 57 | xlabel('Time (sec)') 58 | ylabel('V (m/s)') 59 | 60 | %% Psi-Time Diagram 61 | figure('Name','Psi-Time Diagram') 62 | plot(Psi_UAV) 63 | grid on 64 | title('UAV Heading Angle Variation due to Time') 65 | xlabel('Time (sec)') 66 | ylabel('Psi (deg)') 67 | 68 | %% Target Movemment-Time Diagram 69 | figure('Name','Target Movemment-Time Diagram') 70 | plot(Target_Movement) 71 | grid on 72 | title('Target Movement Variation due to Time') 73 | xlabel('Time (sec)') 74 | ylabel('Position Number') 75 | 76 | %% Distance Between UAV and Target-Time Diagram 77 | figure('Name','Distance Between UAV and Target-Time Diagram') 78 | plot(d) 79 | grid on 80 | title('Distance Between UAV and Target Variation due to Time') 81 | xlabel('Time (sec)') 82 | ylabel('d (m)') 83 | 84 | %% Commanded X Acceleration-Time Diagram 85 | figure('Name','Commanded X Acceleration-Time Diagram') 86 | plot(ax_UAV) 87 | grid on 88 | title('Commanded X Acceleration Variation due to Time') 89 | xlabel('Time (sec)') 90 | ylabel('ax (m/(s^2))') 91 | 92 | %% Commanded Y Acceleration-Time Diagram 93 | figure('Name','Commanded Y Acceleration-Time Diagram') 94 | plot(ay_UAV) 95 | grid on 96 | title('Commanded Y Acceleration Variation due to Time') 97 | xlabel('Time (sec)') 98 | ylabel('ay (m/(s^2))') 99 | -------------------------------------------------------------------------------- /UAV_Target_Following_and_State_Flow_Implementation/Problem_1_Part_A.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Target_Following_and_State_Flow_Implementation/Problem_1_Part_A.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_HIL/Board_Case_2.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_HIL/Board_Case_2.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_HIL/Initial_Data_Case_2.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_Project_Case_2 2 | 3 | %% Target 4 | Vg_t = 10; 5 | gammadot_t = 0*pi/180; 6 | zitadot_t = 2*pi/30; 7 | 8 | %% Commands Constant 9 | c1 = 10; 10 | c2 = 10; 11 | c3 = 10; 12 | alpha1 = 0.5; 13 | alpha2 = 0.5; 14 | alpha3 = 0.5; 15 | landa1 = 1; 16 | landa2 = 1; 17 | landa3 = 1; -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_HIL/Project_HIL_Case_2.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_HIL/Project_HIL_Case_2.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_SIL/Initial_Data_Case_2.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_Project_Case_2 2 | 3 | %% Target 4 | Vg_t = 10; 5 | gammadot_t = 0*pi/180; 6 | zitadot_t = 2*pi/30; 7 | 8 | %% Commands Constant 9 | c1 = 10; 10 | c2 = 10; 11 | c3 = 10; 12 | alpha1 = 0.5; 13 | alpha2 = 0.5; 14 | alpha3 = 0.5; 15 | landa1 = 1; 16 | landa2 = 1; 17 | landa3 = 1; -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_SIL/Plots_Case_2.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_Project_Case_2 2 | 3 | %% Errors 4 | %% e_X,e_Y,e_Z-Time Diagram 5 | figure('Name','e_X,e_Y,e_Z-Time Diagram') 6 | subplot(3,1,1); 7 | plot(e_X,'Linewidth',1.5) 8 | title('Position Tracking Errors (Case2)') 9 | xlabel('Time (sec)') 10 | ylabel('e_X (m)') 11 | 12 | subplot(3,1,2); 13 | plot(e_Y,'Linewidth',1.5) 14 | xlabel('Time (sec)') 15 | ylabel('e_Y (m)') 16 | 17 | subplot(3,1,3); 18 | plot(e_Z,'Linewidth',1.5) 19 | xlabel('Time (sec)') 20 | ylabel('e_Z (m)') 21 | %% e_Vg,e_gamma,e_zita-Time Diagram 22 | figure('Name','e_Vg,e_gamma,e_zita-Time Diagram') 23 | subplot(3,1,1); 24 | plot(e_Vg,'Linewidth',1.5) 25 | title('Speed(Vg),Elevatio(gamma),and Heading(zita) Tracking Errors(Case2)') 26 | xlabel('Time (sec)') 27 | ylabel('e_Vg (m/s)') 28 | 29 | subplot(3,1,2); 30 | plot(e_gammadeg,'Linewidth',1.5) 31 | xlabel('Time (sec)') 32 | ylabel('e_gamma (deg)') 33 | 34 | subplot(3,1,3); 35 | plot(e_zitadeg,'Linewidth',1.5) 36 | xlabel('Time (sec)') 37 | ylabel('e_zita (deg)') 38 | 39 | %% UAV_and_Target_Flight_Path 40 | figure('Name','Flight Path Diagram 3D') 41 | plot3(X_UAV.Data,Y_UAV.Data,Z_UAV.Data,'Linewidth',1.5) 42 | hold on 43 | plot3(X_t.Data,Y_t.Data,Z_t.Data,'r--','Linewidth',1.5) 44 | title('UAV and Target Flight Path 3D(Case2)') 45 | legend('UAV','Target') 46 | grid on 47 | xlabel('X (m)') 48 | ylabel('Y (m)') 49 | zlabel('Z (m)') -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_SIL/Project_Simulation_Case_2.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Helical_Path_SIL/Project_Simulation_Case_2.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_HIL/Board_Case_3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_HIL/Board_Case_3.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_HIL/Initial_Data_Case_3.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_Project_Case_3 2 | 3 | %% Target 4 | Vg_t = 10; 5 | gammadot_t = 0*pi/180; 6 | 7 | %% Commands Constant 8 | c1 = 10; 9 | c2 = 10; 10 | c3 = 10; 11 | alpha1 = 0.5; 12 | alpha2 = 0.5; 13 | alpha3 = 0.5; 14 | landa1 = 1; 15 | landa2 = 1; 16 | landa3 = 1; -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_HIL/Project_HIL_Case_3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_HIL/Project_HIL_Case_3.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_SIL/Initial_Data_Case_3.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_Project_Case_3 2 | 3 | %% Target 4 | Vg_t = 10; 5 | gammadot_t = 0*pi/180; 6 | 7 | %% Commands Constant 8 | c1 = 10; 9 | c2 = 10; 10 | c3 = 10; 11 | alpha1 = 0.5; 12 | alpha2 = 0.5; 13 | alpha3 = 0.5; 14 | landa1 = 1; 15 | landa2 = 1; 16 | landa3 = 1; -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_SIL/Plots_Case_3.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_Project_Case_3 2 | 3 | %% Errors 4 | %% e_X,e_Y,e_Z-Time Diagram 5 | figure('Name','e_X,e_Y,e_Z-Time Diagram') 6 | subplot(3,1,1); 7 | plot(e_X,'Linewidth',1.5) 8 | title('Position Tracking Errors (Case3)') 9 | xlabel('Time (sec)') 10 | ylabel('e_X (m)') 11 | 12 | subplot(3,1,2); 13 | plot(e_Y,'Linewidth',1.5) 14 | xlabel('Time (sec)') 15 | ylabel('e_Y (m)') 16 | 17 | subplot(3,1,3); 18 | plot(e_Z,'Linewidth',1.5) 19 | xlabel('Time (sec)') 20 | ylabel('e_Z (m)') 21 | %% e_Vg,e_gamma,e_zita-Time Diagram 22 | figure('Name','e_Vg,e_gamma,e_zita-Time Diagram') 23 | subplot(3,1,1); 24 | plot(e_Vg,'Linewidth',1.5) 25 | title('Speed(Vg),Elevatio(gamma),and Heading(zita) Tracking Errors(Case3)') 26 | xlabel('Time (sec)') 27 | ylabel('e_Vg (m/s)') 28 | 29 | subplot(3,1,2); 30 | plot(e_gammadeg,'Linewidth',1.5) 31 | xlabel('Time (sec)') 32 | ylabel('e_gamma (deg)') 33 | 34 | subplot(3,1,3); 35 | plot(e_zitadeg,'Linewidth',1.5) 36 | xlabel('Time (sec)') 37 | ylabel('e_zita (deg)') 38 | 39 | %% UAV_and_Target_Flight_Path 40 | figure('Name','Flight Path Diagram 3D') 41 | plot3(X_UAV.Data,Y_UAV.Data,Z_UAV.Data,'Linewidth',1.5) 42 | hold on 43 | plot3(X_t.Data,Y_t.Data,Z_t.Data,'r--','Linewidth',1.5) 44 | title('UAV and Target Flight Path 3D(Case3)') 45 | legend('UAV','Target') 46 | grid on 47 | xlabel('X (m)') 48 | ylabel('Y (m)') 49 | zlabel('Z (m)') -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_SIL/Project_Simulation_Case_3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Sinusoidal_Path_SIL/Project_Simulation_Case_3.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_HIL/Board_Case_1.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_HIL/Board_Case_1.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_HIL/Initial_Data_Case_1.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_Project_Case_1 2 | 3 | %% Target 4 | Vg_t = 10; 5 | gamma_t = 90*pi/180; 6 | zita_t = 0; 7 | 8 | %% Commands Constant 9 | c1 = 10; 10 | c2 = 10; 11 | c3 = 10; 12 | alpha1 = 0.5; 13 | alpha2 = 0.5; 14 | alpha3 = 0.5; 15 | landa1 = 1; 16 | landa2 = 1; 17 | landa3 = 1; -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_HIL/Project_HIL_Case_1.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_HIL/Project_HIL_Case_1.slx -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_SIL/Initial_Data_Case_1.m: -------------------------------------------------------------------------------- 1 | %% Initial_Data_Guidance_and_Navigation_Project_Case_1 2 | 3 | %% Target 4 | Vg_t = 10; 5 | gamma_t = 90*pi/180; 6 | zita_t = 0; 7 | 8 | %% Commands Constant 9 | c1 = 10; 10 | c2 = 10; 11 | c3 = 10; 12 | alpha1 = 0.5; 13 | alpha2 = 0.5; 14 | alpha3 = 0.5; 15 | landa1 = 1; 16 | landa2 = 1; 17 | landa3 = 1; -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_SIL/Plots_Case_1.m: -------------------------------------------------------------------------------- 1 | %% Plots_Guidance_and_Navigation_Project_Case_1 2 | 3 | %% Errors 4 | %% e_X,e_Y,e_Z-Time Diagram 5 | figure('Name','e_X,e_Y,e_Z-Time Diagram') 6 | subplot(3,1,1); 7 | plot(e_X,'Linewidth',1.5) 8 | title('Position Tracking Errors (Case1)') 9 | xlabel('Time (sec)') 10 | ylabel('e_X (m)') 11 | 12 | subplot(3,1,2); 13 | plot(e_Y,'Linewidth',1.5) 14 | xlabel('Time (sec)') 15 | ylabel('e_Y (m)') 16 | 17 | subplot(3,1,3); 18 | plot(e_Z,'Linewidth',1.5) 19 | xlabel('Time (sec)') 20 | ylabel('e_Z (m)') 21 | %% e_Vg,e_gamma,e_zita-Time Diagram 22 | figure('Name','e_Vg,e_gamma,e_zita-Time Diagram') 23 | subplot(3,1,1); 24 | plot(e_Vg,'Linewidth',1.5) 25 | title('Speed(Vg),Elevatio(gamma),and Heading(zita) Tracking Errors(Case1)') 26 | xlabel('Time (sec)') 27 | ylabel('e_Vg (m/s)') 28 | 29 | subplot(3,1,2); 30 | plot(e_gammadeg,'Linewidth',1.5) 31 | xlabel('Time (sec)') 32 | ylabel('e_gamma (deg)') 33 | 34 | subplot(3,1,3); 35 | plot(e_zitadeg,'Linewidth',1.5) 36 | xlabel('Time (sec)') 37 | ylabel('e_zitadeg (deg)') 38 | 39 | %% UAV_and_Target_Flight_Path 40 | figure('Name','Flight Path Diagram 3D') 41 | plot3(X_UAV.Data,Y_UAV.Data,Z_UAV.Data,'Linewidth',1.5) 42 | hold on 43 | plot3(X_t.Data,Y_t.Data,Z_t.Data,'r--','Linewidth',1.5) 44 | title('UAV and Target Flight Path 3D(Case1)') 45 | legend('UAV','Target') 46 | grid on 47 | xlabel('X (m)') 48 | ylabel('Y (m)') 49 | zlabel('Z (m)') -------------------------------------------------------------------------------- /UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_SIL/Project_Simulation_Case_1.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/UAV_Trajectory_Following_Backstepping_Guidance_Law_Straight_Path_SIL/Project_Simulation_Case_1.slx -------------------------------------------------------------------------------- /control project/Problem1_2_3.m: -------------------------------------------------------------------------------- 1 | %% Control System Program Problem 1-2 1-3 2-1 2 | format long 3 | format compact 4 | %% Variables 5 | t = 41; %working time 6 | m_0 = 4000; %initial mass 7 | K = -3000; %thrust coeff 8 | m_dot = -41; %mass rate 9 | d = 0.7; %missile diameter 10 | S = 3.14 * 0.25 * d^2; %missile area 11 | ro_0 = 1.2251; %density at sea level 12 | const = 7 * 10^(-6); 13 | c_d = 0.4; %drag coeff. 14 | k = 6371000; %earth redius 15 | g_0 = 9.8; %gravity coeff. 16 | h = 18690 ; %hight at working time 17 | V = 874.3 ;% velocity at working time 18 | %% Matrix 19 | A = [0 , 1 , 0;(1/(m_0+m_dot*t))*(0.5*ro_0*S*c_d*(V^2)*const)+((2*g_0*k^2)/(k+h)^3) , (-1/(m_0 + m_dot * t)) * (0.5*ro_0*S*c_d*(1-const*h)*V) , (-K*m_dot)/((m_0+m_dot*t)^2)+((0.5*ro_0*S*c_d*V^2*(1-const*h))/((m_0+m_dot*t)^2));0 , 0 , 0] ; 20 | B = [0 ; K / (m_0+m_dot*t) ; 1]; 21 | C = [1 , 0 , 0]; 22 | D = [0] ; 23 | %% Open Loop Transfer Function (Problem 1-2) 24 | [num_open , den_open] = ss2tf(A,B,C,D) 25 | g_poen = tf(num_open , den_open) 26 | %% Problem 1-3 Plot 27 | t_0=0:0.01:41; 28 | figure,plot(t_0,Linear_Hight); 29 | hold on; 30 | plot(t_0,Non_Linear_Hight,'-.g'); 31 | hold off; 32 | title('Hight VS Time (Problem 1-3)'); 33 | xlabel('Time'); 34 | ylabel('Hight'); 35 | legend('Linear Hight','Non Linaer Hight','Location','northwest') 36 | %% Close Loop Transfer Function (Problem 2-1) 37 | [num_close , den_close] = feedback(-num_open , den_open , 1 , 1) 38 | T_close = tf(num_close , den_close) 39 | %% Depict Step Response Of Close Loop Transfer Function 40 | T_approx=tf([0 1.29380197],[1 0.0222002 1.29380197]) 41 | y1=step(T_close,t_0); 42 | y2=step(T_approx,t_0); 43 | figure,plot(t_0,y1); 44 | hold on; 45 | plot(t_0,y2,'-.g'); 46 | hold off; 47 | title('Step Response Of Closed Loop And Approximate Close Loop (Problem 2-2)'); 48 | xlabel('Time'); 49 | ylabel('Step Response'); 50 | legend('Closed Loop','Approximate Close Loop','Location','southwest') 51 | %% Problem 2-4 Plot Of Extra Zeros 52 | figure,plot(t_0,zero_10,'b'); 53 | hold on; 54 | plot(t_0,zero_5,'-.g'); 55 | plot(t_0,zero_1,'r'); 56 | plot(t_0,zero_05,'k'); 57 | plot(t_0,zero_0,'m'); 58 | hold off; 59 | title('Effect Of Adding Extra Zeroes(Problem 2-4)'); 60 | xlabel('Time'); 61 | ylabel('Hight'); 62 | legend('zero 10','zero 5','zero 1','zero 0.5','zero 0','Location','southeast') 63 | %% Problem 2-5 Plot Of Extra Poles 64 | figure,plot(t_0,Pole_10,'b'); 65 | hold on; 66 | plot(t_0,Pole_5,'-.g'); 67 | plot(t_0,Pole_1,'r'); 68 | plot(t_0,Pole_05,'k'); 69 | plot(t_0,Pole_0,'m'); 70 | hold off; 71 | title('Effect Of Adding Extra Poles(Problem 2-5)'); 72 | xlabel('Time'); 73 | ylabel('Hight'); 74 | legend('Pole 10','Pole 5','Pole 1','Pole 0.5','Pole 0','Location','northwest') 75 | %% Problem 3-1 Plot Of Variable Gain For Open Loop 76 | figure,plot(t_0,gain_1,'b'); 77 | hold on; 78 | plot(t_0,gain_15,'-.g'); 79 | plot(t_0,gain_05,'r'); 80 | hold off; 81 | title('Effect Of Variable Gain For Open Loop (Problem 3-1)'); 82 | xlabel('Time'); 83 | ylabel('Hight'); 84 | legend('Gain 1','Gain 1.5','Gain 0.5','Location','northwest') 85 | %% Problem 3-1 Obtain Of Close Loop Transfer Function For Variable Gain 86 | G_open_gain_15=tf([-1.94 -0.01123],[1 0.03088 -0.0002205 0]) %Open Loop Transfer Function With Gain=1.5 87 | G_open_gain_05=tf([-0.647 -0.005615],[1 0.03088 -0.0002205 0]) %Open Loop Transfer Function With Gain=0.5 88 | [num_close_gain_15 , den_close_gain_15] = feedback(-[-1.94 -0.01123] , [1 0.03088 -0.0002205 0] , 1 , 1)%Close Loop Transfer Function With Gain=1.5 89 | T_close_gain_15 = tf(num_close_gain_15 , den_close_gain_15) 90 | [num_close_gain_05 , den_close_gain_05] = feedback(-[-0.647 -0.005615] , [1 0.03088 -0.0002205 0] , 1 , 1)%Close Loop Transfer Function With Gain=0.5 91 | T_close_gain_05 = tf(num_close_gain_05 , den_close_gain_05) 92 | %% Problem 3-1 Plot Of Variable Gain For Close Loop 93 | figure,plot(t_0,gain_1_close,'b'); 94 | hold on; 95 | plot(t_0,gain_15_close,'-.g'); 96 | plot(t_0,gain_05_close,'r'); 97 | hold off; 98 | title('Effect Of Variable Gain For Close Loop (Problem 3-1)'); 99 | xlabel('Time'); 100 | ylabel('Hight'); 101 | legend('Gain 1','Gain 1.5','Gain 0.5','Location','northwest') 102 | %% Close Loop Transfer Function Variable Sensor Gain (Problem 3-2) 103 | [num_close_sensor_gain_1 , den_close_sensor_gain_1] = feedback(-num_open , den_open , 1 , 1) 104 | T_close_sensor_gain_1 = tf(num_close_sensor_gain_1 , den_close_sensor_gain_1) 105 | [num_close_sensor_gain_05 , den_close_sensor_gain_05] = feedback(-num_open , den_open , 1 , 2) 106 | T_close_sensor_gain_05 = tf(num_close_sensor_gain_05 , den_close_sensor_gain_05) 107 | [num_close_sensor_gain_2 , den_close_sensor_gain_2] = feedback(-num_open , den_open , 2 , 1) 108 | T_close_sensor_gain_2 = tf(num_close_sensor_gain_2 , den_close_sensor_gain_2) 109 | %% Problem 3-2 Plot Of Variable Sensor Gain For Close Loop (Problem 3-2) 110 | figure,plot(t_0,sensor_1_close,'b'); 111 | hold on; 112 | plot(t_0,sensor_05_close,'-.g'); 113 | plot(t_0,sensor_2_close,'r'); 114 | hold off; 115 | title('Effect Of Variable Sensor Gain For Close Loop (Problem 3-2)'); 116 | xlabel('Time'); 117 | ylabel('Hight'); 118 | legend('Sensor Gain 1','Sensor Gain 0.5','Sensor Gain 2','Location','northwest') -------------------------------------------------------------------------------- /control project/problem1_2_3.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/oygx210/Guidance-Navigation-and-Control/c835893057a4aaf4808b9159a7ccebcc04983be3/control project/problem1_2_3.slx --------------------------------------------------------------------------------