From 81426af0ce00a5a8a81f46b6ea8f6095e20240d4 Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Tue, 21 Apr 2020 00:43:27 -0400 Subject: [PATCH] Nonlinear Plots - Generate all plots via nonlinear system - Remove extra matlab and simulink files that are no longer used --- src/LQR.m | 310 ++++++++++++++++++++++++------------------ src/LQRSim_Goal_1.slx | Bin 44733 -> 0 bytes src/LQR_Goal_1.m | 284 -------------------------------------- 3 files changed, 178 insertions(+), 416 deletions(-) delete mode 100644 src/LQRSim_Goal_1.slx delete mode 100644 src/LQR_Goal_1.m diff --git a/src/LQR.m b/src/LQR.m index b4cfd2d..c3d8e36 100644 --- a/src/LQR.m +++ b/src/LQR.m @@ -1,14 +1,9 @@ % Clear workspace -clear all; -close all; -clc; +clear all; close all; clc; % Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf -g = 9.81; -m = 0.468; -Ix = 4.856*10^-3; -Iy = 4.856*10^-3; -Iz = 8.801*10^-3; +g = 9.81; m = 0.468; Ix = 4.856*10^-3; +Iy = 4.856*10^-3; Iz = 8.801*10^-3; % States: % X1: x X4: x' @@ -74,46 +69,39 @@ continuous = ss(A, B, C, D); T_s = 0.01; discrete = c2d(continuous, T_s); -% Check if this works +%Check if this works impulse(discrete, 0:T_s:1); -% We should see that U1 gets us only translation in z, U2 couples Y2 and Y4, -% U3 couples Y1 and Y5, and U4 gets us Y6 +%We should see that U1 gets us only translation in z, U2 couples Y2 and Y4, +%U3 couples Y1 and Y5, and U4 gets us Y6 %% Define goals -% Goal 1: settle at 1m height <2s -% LQR drives states to 0, so we redefine initial -% condition to be -1 in z direction such that -% controller gives a positive z input as if -% quadcopter drives from origin up 1 m in z direction +%Goal 1: settle at 1m height <2s x_0_up = [0, 0, -1, ... - 0, 0, 0, ... - 0, 0, 0, ... - 0, 0, 0]'; + 0, 0, 0, ... + 0, 0, 0, ... + 0, 0, 0]'; %Redefine origin! -% Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot -x_0_pitch = [0, 0, 0, ... - 0, 0, 0, ... - 10, 0, 0, ... - 0, 0, 0]'; %Pitch of 10 degrees +%Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot +x_0_pitchroll = [0, 0, 0, ... + 0, 0, 0, ... + 10*(pi/180), 10*(pi/180), 0, ... + 0, 0, 0]'; %Pitch and roll of 10 degrees x_0_roll = [0, 0, 0, ... - 0, 0, 0, ... - 0, 10, 0, ... - 0, 0, 0]'; %Roll of 10 degrees + 0, 0, 0, ... + 0, 10*(pi/180), 0, ... + 0, 0, 0]'; %Roll of 10 degrees -% Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds. -% Redefine initial condition to be -1 in x, y, and z direction so -% when LQR drives states to 0, it is as if quadcopter drives from -% origin to (1,1,1) -x_0_trans = [-1, -1, -1, ... - 0, 0, 0, ... - 0, 0, 0, ... - 0, 0, 0]'; +%Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds. +x_0_trans = [-1, -1, 0, ... + 0, 0, 0, ... + 0, 0, 0, ... + 0, 0, 0]'; %Redefine origin! %% Finite-Time Horizon LQR for Goal 1 -% Cost matrices +%Define Q and R for the cost function. Begin with nominal ones for all. Q = diag([1000, 1000, 1000, ... % x, y, z 1, 1, 100, ... % x', y', z' 200, 200, 1, ... % roll, pitch, yaw @@ -121,21 +109,32 @@ Q = diag([1000, 1000, 1000, ... % x, y, z R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque -% Calculate number of timesteps. +%Calculate number of timesteps. tSpan = 0:T_s:2; nSteps = length(tSpan); -% Determine gains +%Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); +FiniteLQR_Goal_1_K = K; -% Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B); -% States are relative to origin, so we need to add the reference to the -% state to get global coordinates -xlqr(3,:) = xlqr(3,:) + 1; -% Plot -plot_states(xlqr, tSpan); -zd = diff(xlqr(6,:))./T_s +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); +set_param('LQRNonlinearSim', 'StopTime', '2') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); + +state = [simout.x'; + simout.y'; + simout.z' + 1; + simout.xdot'; + simout.ydot'; + simout.zdot'; + simout.pitch'; + simout.roll'; + simout.yaw'; + simout.dotpitch'; + simout.dotroll'; + simout.dotyaw']; +plot_states(state, tSpan); %% Infinite-Time Horizon LQR for Goal 1 @@ -152,103 +151,142 @@ tSpan = 0:T_s:2; nSteps = length(tSpan); % Determine Gains -[X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []); -[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B); -xlqr(3,:) = xlqr(3,:) + 1; -plot_states(xlqr, tSpan); -zd = diff(xlqr(6,:))./T_s; +[X, K_const, L, info] = idare(discrete.A, discrete.B, Q, R, [], []); + +K = zeros(nInputs, nStates, nSteps); +for i = 1:nSteps + K(:,:,i) = K_const; +end + +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); +set_param('LQRNonlinearSim', 'StopTime', '2') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); + +state = [simout.x'; + simout.y'; + simout.z' + 1; + simout.xdot'; + simout.ydot'; + simout.zdot'; + simout.pitch'; + simout.roll'; + simout.yaw'; + simout.dotpitch'; + simout.dotroll'; + simout.dotyaw']; +plot_states(state, tSpan); %% Finite-Time Horizon LQR for Goal 2 -% Cost matrices -Q = diag([1000, 1000, 1000, ... % x, y, z - 1, 1, 100, ... % x', y', z' - 200, 200, 1, ... % roll, pitch, yaw - 1, 1, 1]); % roll', pitch', yaw' +Q = diag([0, 0, 0, ... % x, y, z + 0, 0, 0, ... % x', y', z' + 1000, 1000, 1, ... % roll, pitch, yaw + 10, 10, 1]); % roll', pitch', yaw' R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque -% Calculate number of timesteps. -tSpan = 0:T_s:2; +%Calculate number of timesteps. +tSpan = 0:T_s:4; nSteps = length(tSpan); -% Determine gains +%Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); -% Pitch Goal -% Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B); +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); +set_param('LQRNonlinearSim', 'StopTime', '4') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); -% Plot -plot_states(xlqr, tSpan); -yd = diff(xlqr(5,:))./T_s; -pd = diff(xlqr(7,:))./T_s; -% Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B); - -% Plot -plot_states(xlqr, tSpan); -xd = diff(xlqr(4,:))./T_s; -rd = diff(xlqr(8,:))./T_s; +state = [simout.x'; + simout.y'; + simout.z' + 1; + simout.xdot'; + simout.ydot'; + simout.zdot'; + simout.pitch'; + simout.roll'; + simout.yaw'; + simout.dotpitch'; + simout.dotroll'; + simout.dotyaw']; +plot_states(state, tSpan); %% Infinite-Time Horizon LQR for Goal 2 % Cost matrices -Q = diag([1000, 1000, 1, ... % x, y, z - 10, 10, 1, ... % x', y', z' - 1000, 1000, 1, ... % roll, pitch, yaw - 1, 1, 1]); % roll', pitch', yaw' +Q = diag([0, 0, 0, ... % x, y, z + 0, 0, 0, ... % x', y', z' + 1000, 1000, 0, ... % roll, pitch, yaw + 10, 10, 0]); % roll', pitch', yaw' R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque % Calculate number of timesteps. -tSpan = 0:T_s:2; +tSpan = 0:T_s:4; nSteps = length(tSpan); % Determine gains [X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []); +K = zeros(nInputs, nStates, nSteps); +for i = 1:nSteps + K(:,:,i) = K_const; +end -% Pitch Goal -% Propagate -[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B); +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); +set_param('LQRNonlinearSim', 'StopTime', '4') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); -% Plot -plot_states(xlqr, tSpan); -yd = diff(xlqr(5,:))./T_s; -pd = diff(xlqr(7,:))./T_s; -% Propagate -[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B); - -% Plot -plot_states(xlqr, tSpan); -xd = diff(xlqr(4,:))./T_s; -rd = diff(xlqr(8,:))./T_s; +state = [simout.x'; + simout.y'; + simout.z' + 1; + simout.xdot'; + simout.ydot'; + simout.zdot'; + simout.pitch'; + simout.roll'; + simout.yaw'; + simout.dotpitch'; + simout.dotroll'; + simout.dotyaw']; +plot_states(state, tSpan); %% Finite-Time Horizon For Goal 3 -% Cost matrices Q = diag([1000, 1000, 1000, ... % x, y, z - 1, 1, 100, ... % x', y', z' - 200, 200, 1, ... % roll, pitch, yaw - 1, 1, 1]); % roll', pitch', yaw' + 0, 0, 0, ... % x', y', z' + 1000, 1000, 0, ... % roll, pitch, yaw + 0, 0, 0]); % roll', pitch', yaw' + +R = diag([1, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque -R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque - -% Calculate number of timesteps. +%Calculate number of timesteps. tSpan = 0:T_s:5; nSteps = length(tSpan); -% Determine gains +%Determine gains [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); -% Pitch Goal -% Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B); -xlqr(1:3,:) = xlqr(1:3,:) + 1; +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); +set_param('LQRNonlinearSim', 'StopTime', '5') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); -% Plot -plot_states(xlqr, tSpan); +state = [simout.x'; + simout.y'; + simout.z' + 1; + simout.xdot'; + simout.ydot'; + simout.zdot'; + simout.pitch'; + simout.roll'; + simout.yaw'; + simout.dotpitch'; + simout.dotroll'; + simout.dotyaw']; +plot_states(state, tSpan); %% Infinite-Time Horizon For Goal 3 @@ -267,22 +305,38 @@ nSteps = length(tSpan); % Determine gains [X, K, L, info] = idare(discrete.A, discrete.B, Q, R, [], []); -% Pitch Goal -% Propagate -[ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B); -xlqr(1:3,:) = xlqr(1:3,:) + 1; +K = zeros(nInputs, nStates, nSteps); +for i = 1:nSteps + K(:,:,i) = K_const; +end -% Plot -plot_states(xlqr, tSpan); +%Simulate nonlinear model +[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); +set_param('LQRNonlinearSim', 'StopTime', '5') +simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); + +state = [simout.x'; + simout.y'; + simout.z' + 1; + simout.xdot'; + simout.ydot'; + simout.zdot'; + simout.pitch'; + simout.roll'; + simout.yaw'; + simout.dotpitch'; + simout.dotroll'; + simout.dotyaw']; +plot_states(state, tSpan); %% Helper Functions function [K, P] = LQR_LTI(A, B, Q, R, nSteps) - % Set P up + %Set P up P = zeros(size(Q, 1), size(Q, 2), nSteps); - % Initial value of P + %Initial value of P P(:, :, nSteps) = 1/2 * Q; - % Set K up, initial K is 0, so this is fine. + %Set K up, initial K is 0, so this is fine. K = zeros(length(R), length(Q), nSteps); for i = nSteps-1:-1:1 @@ -294,7 +348,7 @@ function [K, P] = LQR_LTI(A, B, Q, R, nSteps) end function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B) - % Set up for propagation + %Set up for propagation ulqr = zeros(nInputs, nSteps); xlqr = zeros(nStates, nSteps); xlqr(:, 1) = x_0; @@ -305,21 +359,9 @@ function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B) end end -function [ulqr, xlqr] = propagate_inf(nInputs, nStates, nSteps, x_0, K, A, B) - % Set up for propagation - ulqr = zeros(nInputs, nSteps); - xlqr = zeros(nStates, nSteps); - xlqr(:, 1) = x_0; - - for i = 1:(nSteps - 1) - ulqr(:,i) = K * xlqr(:,i); - xlqr(:,i+1) = (A*xlqr(:, i) - B*ulqr(:, i)); - end -end - function plot_states(xlqr, tSpan) figure(); - subplot(1, 2, 1); + subplot(2, 1, 1); plot(tSpan, xlqr(1, :), '-r', 'LineWidth', 2); hold on; plot(tSpan, xlqr(2, :), '-g'); @@ -332,7 +374,7 @@ function plot_states(xlqr, tSpan) xlabel("Time(s)"); ylabel("Displacement (m)"); - subplot(1, 2, 2); + subplot(2, 1, 2); plot(tSpan, xlqr(7, :), '-r'); hold on; plot(tSpan, xlqr(8, :), '-g'); @@ -343,5 +385,9 @@ function plot_states(xlqr, tSpan) legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate'); title("Angular Displacements(-) and Velocities(--)"); xlabel("Time(s)"); - ylabel("Displacement (deg)"); -end \ No newline at end of file + ylabel("Displacement (rad)"); +end + +function [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(X0) +x0=X0(1); y0=X0(2); z0=X0(3); xdot0=X0(4); ydot0=X0(5); zdot0=X0(6); phi0=X0(7); theta0=X0(8); psi0=X0(9); phidot0=X0(10); thetadot0=X0(11); psidot0=X0(12); +end diff --git a/src/LQRSim_Goal_1.slx b/src/LQRSim_Goal_1.slx deleted file mode 100644 index 264ff9ab2b6477358ff05e4fdf871a1fd9ec62ad..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 44733 zcmaI6Q;=w3mn>MeZC9PLZQHhO+qP}nwsFd~ZQFCYZ_GsheIw>&KkT>sGM92Kc`0BJ z6aWAK2mr{0MEU9tm2fa10Dy8R0088Ff3<|{Y@JPPo%NJG>`k0>Xx(kBM^f$VHs}#X z`y`XwkQuH}bvq!L<9iAC-_U@`bA%a@*MMNQ7 zp^tLY$p^?qi^%-RaDW1$(V3yXLp}P-3m2A$!N!>n7bJfiUUtY~DnEfR?fR*5AwjKN z(#9EARAsiYgIT>&O$t<<5-3DK=(TW;&kE)R9U;`Q{3fXW8vs%V;CvBBCck57vo-NY zw_FjbM^c#*o1==QpyHQW``=Q)~6Inh1t-$qyTLk*rp~K&@v*}ZFkX3`%eXMH3G~UPIrl*YHXK^w zZ9?FS=HJCZyAeKOMtk{R7n-_0$zk+}u|H($r(Zt$`j`qL4B})o&LG%dVcXfgvyin~JEJj`#WM1?PoP7N?TloR4 z#n)Re*ECqT^@eNS!G}Mu&}!*4aD`C3^4PV;k_YrefRd$*w2#T%b$MiNB=l&;U@3{| ziNsh))F(}9)%~4#N_QI|w8ELmC@U-Q$yloS0LYbCB~{ibX!_eFGZwJb!8jI3B5lfT zOyX=z>LwH#=v)m6{ZvTR0;lFmM00{*XyxA(AuUrn-|C0I08iYR^6-{Q6soiwM_9w- zChcn~+Nn=f**O^vD#aFY zOF3sukSPW&N7WsWA7={Bx$SGuTTg85O)M{>pYEP1Be_hZFVYs?^EX@Olk?G{IkGC* z**#UR2k~T>1s>e$$mj4V(?=jO-ju@v_RoFdBL#NQcZ8x7?BJNa2^KP_` z?ZktDiY)*GT)>hBj`V9OY}5gUr>Z@Q*$6~CDR9>(J6=~#s_T=JT zt~2JNgfq{EZ<43nHW5UX|MP|>Nn2x7J75rUiZyI>NA<|zB4lN<>&GvE==SgNtjhN@N=npRcO_al=^4wg_xeg2Ch7=tIaIMuRBqk?}sY-FoTX)WK&pj9E&vKeQsZ?tE z!~L8QyHniY{}aF8O6F?kKistc@RR@l;a4=VHZgEAk+3zj`>z*piIsv6q(=z8{fbQS zDnJpZr#K#+hQ47MGc~qi!{nmUS4nh z)P(H2r_lLoK|ep;-2Qoc`oHkEk+TyA!vO&F!2y{1-&$ zrKs!tVwaWAbFd(V`n-Joor($`1PDhQ+AnHas;=Ug5->5z{wh>iR-Hg8+QtwK4t*?LBkPJ60V4{{;SbLWYi4lQfyJtp6 zRbyT^uT1e#6_jkkRl{1q85bk(%M%!cg_oC>w`C~bI@y#?msk{Rc_j&wwJ`U0@yp)| zg*|)#$OD0?Zk=H4xV|m@3t)Q#>ew%ST4PA2q_meiTg8>&SYz*I-{AY(src2fu(UP- zku*Fs5XPN!Y!xz0lMMe>x_$=~CIC{X+`bQqU+O9Xc9g>z-bB1&baz(asIjSO(rFRw zUn@Xf;Ca|y3Q+7MH|*ZJxV8E)bs`4V6L5=>lMc~@?d>EZDr(BrxZxW;tA*F;=Lkh6 zxlbOzLZQ1Z+7THd)b4eWZ90eM$v5`rPlQ#9jl9|#Qsca!@>+FhXbL{!WAk(XeC=;@ z@9s&_@*Z|Vg=$}sc3F=e(^7tK0by;-luBdq($ZYX0JEb!Szz*IF`^S@HUmm(PL9pT zNn)5GTyU<=lT)NWAf0c|d<=7+OMevIV49X8t%#^-X6xGyVy`g%80qOcSBKvL-B&%< zixQ4{E`z2ReKg!)dSWulkz2D>-vtW;3o0P}je8zOT}e@p#v|L2A+IzS#|E@nvg+^S z>FlT|I!Skn@5a1WO*fv+lWcu!Z!X3}taYA^vyuo0J97sOr+yHE8?~xGO*t6!C0$d(#T{&vs?3C#Cf1-UAw-YM`3Gf*v`afcRCRxBcm-& zx1&w--!9wCl$4SXe*P+|KEJko<+~K}Z3^7)9bDWWO4=G=LocqLi3|cNhf}zHBMun2 zj0p`&3V2O9}KHmjVmfOCn^uLmpn$qe;CMj9;{YV<4o6 zrItOOhXb^~qu`C6sd$=0E_~b;Q);_XTS>T}Vhz3Ohgtp%wmN{F9f*&qHadByRdC5K~m_YJuTXokQ z3c%#7^$#+5U?yWo3V4Kv@Bux;!r7;DU0mpZIY(M z83r^N0hTRO_%Z6by1ti*tiHRz)ochb3e>J;H0_i5Dep_`?=gDZP8}wP%b)!S@>OV8 zF=Y~g;ZfA2)<j9}+E-<+T9< z_Sa1kF`~L<5MIZ0alcqI$sIpe@#5|QgzmpzYg8w}bri0OC9trtK2)(#pY-DGCiDH$ z+B`Om0c6<+Z*$vJH)mg8mnBS{)NS1dBI)%B{+RXSJ?g~_OiWUUD!57V3maUV6wgL1 ztc;Cdkr&-?o(Js{TP8bz7h1sdY}W~Wx=PWJVO?IfwzPluzIikg91{LOFDPUWGfngl z^l)k{p${Tc#!Pk_k%=E^r$kMX63W4#=YI|0sxk{DL+8x$ZLs_TtWM+YSB$tFQ~;Y< zP=<7)aZ+#`b7P7gwcV^wv?vyN8a`!`hlX?05J#-CjNpvgp}{>AFX+O$&L+clW5}24 zNgXnsl&3W97q-2ez4l86gtK7mFyk^YS~xD!TT)ugCP~K;0_BmOhOA9*3Nco9*m2NQz78|JBps1R!AZ6$kn^jwup>` zH0%h;Wt9{W5pmFpc|!YC`|Oa-XpAvv--|fq{oAN%mLk;=(!s$KimP22|?k$!2qLyxzmCGfzlKMO9u`vCTK}%vY}5-L$&)wSu0Kb zjYpuj&GKTTHwKXmn5ic4=w9U7FuUBc1vqx#A3&aK9Ty<>#Kes*wN$ zs6h#4{iZN0rnLXPjhS{T1#G=T^Y{At`u)T1XFvyff_n-ePlcJeoLYGO85TzL^yI+R zjYnoO#wZ^OC!AC}$K2a+@MPBTH!crWVd9Uyycefj zVEc5a9a-RS3K1KTmaeHTSudNr>&gKUmzS?dNK*w;Ur#`VBZBMDB{43pPRikvjT>DZ z?dB$BO42GFQ}DY2SSkj}HVqc=ZI{PD)MzckqB@PAgkRN}8%Fylzi-l@PAXM-P}mjA znbeQ!K~8E^K5Sj=lMO-PS?;o)%0t?1M28^W79xA0^UKATju8}~0AXVr2Q9rkAguK= zFNIqHtBq(=O%XB-(A0TlL!zcnH5AM%m(U^^ASY09oV`1vUf)pWA5&xi$;a;0kEh@w z&44Nob7qY&R3;0Our9LHJ)GF;U<8nzpVqBDZTZ>QYGw)sXviVst8^6E+IMd6aRf;X zc{@^eH9{?0a>q73Tq0u$(>kll=osFTPIt$?;j1v?%yJWi8N%iu4!aG_!G@0&+|MDG zhnO@}4gsourYo^4l+zLY-+)_Hq5P(#vw5>Vk$~u1GYDX)+$X<23Q5KCEfB9w9KX= zmW(Jz?;Iwo0w7#MgQw!@<9dIO5*7vBWG?*4p0!HtkETPNiwg4@uwOLVb?Q>45j%m1 zww%*z(&f|l)iD4yYz&OE$M)((*UN;^06;1tF$eMQ3y~}@Jwo5nuu)QP?Xt4ZI^P4U z^O(ss&+6izFNdbYJq%oIVlfp*?7OC%)9N7(v(2X?*pkBb+Oa&e2auQABJ$y{zN5P) zfp%jdf9rm(kSOKHF3{;@@GNEZ?$i#necuvQSgvQp<&m;QaeG~ogz$UGvWc0Qnb`Rl zAz!cHJNFSGECLSYq(Enz&nCj<)^F;3*N$gXRSn;JhmFxfYdy|gH1fS`7!9>@;DBsE zFhCIKR0l1%@kEA!yj+}^1N-EZr1`_c(5T7Dz`5Y3zFY{_cNW+Mta8N@KF&#Yk@mOM z(DV)+5Z}Qy;DS5f3oq$1B82Yb=9U)N^{J-aqjn#!WEsDp=zb(t0w=Y=DGDjI`~&f)!G)X?{z834 z?>DVZE>Xd)kN;4uVTY2D=?u$MDKvMwYVzXz?K8{Rv`R{O&*N>t!@)U9fLp~cR06=@ zRcCJW{#OQh7I~g3h-M+3kjsQdi9d9ZpvC0$o7g9lK8uoUE5p+@kuSep+oG7sJ)!^l}Rd{ z`*i0Kt^*PPr+2wxee83I4|6FB#71MI*6)+k(p*!sP)Z>#OQ-J1rfbNV0+9|pSX!+E zEX-qG-vLi`Jggd3l>{t(^98l7O)O9PZ+^+W?7{Edt@?Mmsh>|gCTA#Pb>aJ{-YC9-I`uMu4=f{EqPo=t{(%1H+@5fhuF&?+O6vg!0Koh=w>nwaxL8}*TG1&P znVT59Sepp_v(^6F7FQX!-K0n8Ii-TxwunGMZ>UGjmq`=a6iRjKH!+58Knr)ydb*U- zNNsDEfi6CiIGQqlJL%cCcYRl{A=}M&GEtf(=9VdhTqXzBYJ0f; zE>yaHpvMqPQq2Hw@$ag2el$P#`6P?Y(GV;tO?jBSm3iGOEv-1H6hDwjt&FUb5JYm3 zL!x$wWGH2hc!(M|cA9Fg9E8V}-eqYC6XUy=qnRplsx@!aO>@Q5pei0%Ba&_rPDa78 zsAe%71Y+SPmHs;oxF6n1l~wyBr)B~^8YcK4cWn6XqGNH!OLb~?{}{e6S6^b+iCxE? zJ@n*zi|1fb&@r6BPgww>0EJ>fLrw|9AnD(zy-!mc-{;IG*P`Jnqw@oh^=S*+^ft^+ z5CNZWEu*verge-rLwI>jTs&oNyO2zrX^%qy8Nf)5N!dJ_C_e;Z;3aX>=0vKi9Rf`|12^A^`*6f7%E zCLM@6m}(^~fK5Vb5^hh_P{=+%qFWNb2F|%H#SING<)P13Fhwu{7|&YGAFp7gU3u;}xJHeaI(Ady zXr~tAVXtNL@_E?ao0x?*RTp{g^M}AIl%U@5F*n!1C7T(qDR(W0?$hsMcEqCn(<`up z(%2G;UmGEEkU(GD1V%oAUhyor1oNbi@J;CbSj2b8N5*7uXbt^0=(Lm^=dIVLtx63A zsE>M>dIu(U+dFa$kOl79A=plL@NMfD4&I^LFXA!pKh^%habmpmPwhdF|HnN3ztk>b zY~gI@_+NUjQQdURqDSzVRil4m5kwyr!Ofa#miSoWC7D(#OG4~ilLqhrXp`vc1%e^7 zHROu9#>~pmxyHl2#$)Jmr*=F|tGt*>c0%|1>!M5%JvA}1)${2KYxqjUk+tm6)Tzd3 zCCw5uTHADR5J0!2ld|mOw$5A?hp=K;0d<4gt?|RUEOZU2)GtWvo^dZS5>-Wu*i8t* zTCqv>NMu*I7K>t~&Yv_xaFHin@Txpg$_pIJCD4yb#)hPSetmx#X z7k*j2nvvZVFbr4<0f+pHCAUBYRrl(#OWh5aY6{grenN1N34{fOZ!gR;Cl0|kLV--=xSzg#L#{S%ro_s$ z#utaDMIqr)Q(fePb6>sKh98oeE<#ws{}!yFOaUleirBBHj{_2!w1z*$jE=KmWz>h- zQzwBCDGrDlI{bpw@&u{~6f8L0a*~j2%-<*ATfwY%Iy-O+$Q$$-+Yq$|xTaWyrzgJG3FE=nM4geXrcShz+Xk-{JYexuwSeM*$7K})5QYaU*Dt!Yx) zxDh|sss(OzS?A6qIVro2ec5>gn^$aAkh!Hi4!-1LTF+JnD5dK+5#wH-RI0g&xV^Dh z-fC;O>h`NN)_ZePc!``Jr|{|X=L7$2Yk*I0d(h?OIUE-U=fuy&)(e&ge^F^0MyaO3 zvw^+wFm9p2DDFeX>kD5b&lU-;YwO~OKAx*>UTWbPuUxDL9OeR2?EBp0-o&8EPRag( zGv(`lrT#@pyxPfscI^cO007DVO#T07yJcu?XJln;VPNKHVDn!A@!#97o)tZ-e^mhB zuFmkO_pi*@k3w6ra|l1z#6rVZzPOo|@Cs&5H}&U}Qe*sDEUvov_gUBDHoU~&-i}Y5 z^fPq!1#@IqmPic{)xrfhJ_?KG4SvQ(s0 zFFoJ=Y-6~Pe8g0JAu~iBxu>r^61t{Gi80ClM5f@pkz^;)ge(R8KQI?2zEbGuPG%k`Lxg|FZd=}@b%lQ{JduFIwE zc|EQ>Ra%&dw*}O)bonOq~B;-~PYVU=lZA`EQRCsyA}hOO9Ikx3)a{H>DhgoluSvMXy>m9K#F4M=^ zfmP09mea`6hx%cry8D%NNt81SNm`o6@=&<4y}1$zkF&?VSGw#D{-3aO-VmbA{}qGB zf0)t#Cu~DwV-r&Y7i;JL-XvaY+Sp;UA^q&=HLSsh;d#bh@8H_gH9~1LA_2FOsz-t{ zLjYO2H5XAY4eD%m+oSxEQe)MS}hx*FRLvqsBHWI7(%9+QNv)}@BYOBS}1Nq-W%B} zwW)XtjG!+ihHfm!2%eg7LTZeI=MIDWmRm$U%P*w)2XWL zm_xC?8Vl7~+=ZKbwHkS|-DlybH*PiMVE*~^g6BA$ORGqh;XcaLK{`|B5TT5eCl6qk zdz6a~?^J=5W>`#58<4ZIg|I3lYMn2eSVfAQq9sEeL#5#q$L8j5G8|5GIYc6x@>jyg z3!WNZ{9b!kwo!NIIqYms;$&YC7rTiJ}!eqvkcJJ`yw6xwjUQ_go(0=K)6$kkAwO?Wl zqb<`s<$HfiO*;QY9Z=vbrB}NMcp&v(+6M(yjKf~T#Rkv2tlUw4ozCb;^C=zQ?fj02 zfM~jlug(nE7pD+O)>MTJuby4t&I8qgjue)24B5|Zt4R;qQoU#da7$ zZGT)MXv#RlP^|iPUBRKXHyhXGAd!~H3<}%lxcmm|&#mNh1j}pi%hYpN?`NitNz_mp z<+%Na&x^(P(>}b!i}CmQCHaA_qaCWM(Q5I{90XEH=@x& zGh&D;QVGpmNrK>F7%vLGB{MzKX{dH0>oO&W5b0LF`UY@W5!1&q?nIogzBUWkDM>>~ zCy$-r<(DBuJus|qzkU=P+C|J&QI{ip{Ie~6`$mP)-vzZHtDjr3M}=0b8la)m5Y?#k z5O$21TkaVHp){%O=+{ncQJ57|X0!obC*w4l*86QG1L@kIMN!yt8;coNR5wa2+ek7o zZI_e)ypaTyEZ7=#3k?o}$qC&E#>8_DrFJ%7_g(}&~<;fBS~e0l-Rr$aw-)G zn;2Asn3FZ}gnH@dyMeTL6K#IOXu7m8lBw3p_e`3U(4QmP;aaie5&3BERRycH3<7HP zz?ztx{t+WPP6st=k{Pi4fWCrM#14NxzSSa0D)=3!IQo6x-<*I~TTsTb9MQ|#us^W9 z5xmq_4U?nmT^jg-d_2)N3u-GKnU_#m1`TcQxh!7#0wt)0OBn%7~E?Uvgb? z@Q(hm-ex*@?F~ZeSqUmh?r^S%_=N{eR@Tl^L5|naCsH6FnoJ%UqwsmjemjP`+4Qn@ zP=#uYy6kPIEj4CA26s}W%7(mFr`es0gC~RC^BFLNOjdzM{f@WpD(@oIpTBX){2UuI zdGVoN4^qRSL9(L4T!>0suMUoOlCF}2v`CiT;E3m`Msr7i2^d&458hy2(85%ZYpbk4 zkaGy}Isb+C$rSfQ5o@V#4;91V>W6!}@Up#kL#gO#&xRart=LxwKHO`XKMvQ4w^`v6LnNufqj26U!_=!^t}+fq^4!?36hT)Jbmmn(Yad~%=|>RG)^7^5D8+XjBUpP(SF4N>^Ww6LF`h@90yZMd7FaCVI4P_e z(LeS?pd~~m&|N`@p%UON;o-n+W^8j)W!@h7C^HYw7ko?!LIvy2B;+Oj;Bws!zgy%< zycL4LBdtD%eG5sX(Xz&;r0bZ)Z_D(a$w%99AxfZqT!vmv@ zM#Im>a8!;dn0YfX*^I zI};e&{OmyLt()eE5X4%^;uRa$CV%0wfBcYK(9tDsl#+Vg_B3>Eib=6IL zJQn3r{8gs4cOT~TOSEV=Pw9j^Kw0??+U;LAj=NV7PA*5gr9JkCudC1NR#XZRFzZqM zZ?btXr(Pw|4%Zk3!3kuZ+6;@t{^0XCvhX5o0?85q-DHrCosxHrV&%L*Et0$?; z_;}_3JB&aDzjSz@7a_iisGvR1Kpg1TNS;!HDbl^UQvD=?n~k&y?3%o>Jv2ig8=wp# z2Rm-}FFOWIYy;aFh6FKtszsxrXi7^KLzG&0XabL?e7=Os6&gb=WKypCco$*d@&sbLp_grU$}T{A6v>V7D1Kt$tZ247pEC3|Y8 zCTJT6+iYzZ{7n76{zG>*O)Ys4=y5$j?pe$w>?}4>J%QvVN#g}eFlU zrZ#j^QNg>^GBFD(cjlbqfVjUCDOp&U{;82&}UEfxUgOlvr|u8kY4C59}I6!+Zmwh%BAk? z-BZ5Bj1VM+{r9EZt(B&j8SW33jwlN5!i6yYwV}RrHWb-Wm7zIEEC>3AYntm*U@d6^ z*Ho4mbz3wdjJ)!a&QfsSSTw!FN`YUHNI*EIX1_|ABd`u+#q07~%8}!Llu++$RySaOrJ(uPRodQ%H0qGUh@gFA!10oNa7Zd}ItJuZi#rFtGe5 zN%`7Sg9QAV8Oj51XvzAm^m(egkU=zKHkfV5`Iu<_R?YViamn`4z!CF;ogHVfufEom z3zb!avI)qd-@$Au`j;)RB-*dUzhm51Lel9F_|ru)Zka8aRsmE;@Dv0A@^*nnh9QZj zCz8FX={t6bp4S7s1^f`Kgf);6F&d|p!1M{?{`FNG?l_s6UU<64TV$+Qe;G!)G%Y=t zSEdhf$Er1umL2XG%`(BTAxSc!@=Bi*JXV%Y#ULg#WHMg>?fp@b16JSFQ91OgPoRS+ z;hlICVeU_Ib@#q=7UDNPGaj6IXd0KKJK+*CB?A6l8DFv0(3*wB!?;e&GZS34b?Ham zItU)B!&AAF8upMP$&&NUW3wBqLFC+`n(#5p!Y5L3i7_nUX90*KuM*f9>IsFW8x;MU z`z?u|NaBp7Wvksl;5y@-Vng1A)v;~smKi$~!CKf1p%40q3S4kW^x;X!4 zJ@c&@;t1*KzQj2?u~*Z`24v3KgFEAL2~(xS^;|uX{>FS&&}2WLG)Z=P`JgUZgYqSh z>9p^UGSAtqB%lbYQCEwHFW@{8L!9)U zw>`hkA2X?PG2$3)I=u5*J^lut*(*8Mr*={Xh;-;N2t?NIa+6Eh6Idu(zohSZIAPcD ze{$M_TAo=-QB~yAX6Pi=6L=5<*72KlkbpvBtMXAf2kC9;hw8=6nGF*tqgYplZ$s>Fr@nb;i~B(4EZ$IYRgB`&)~@79d8P?AZConQVADqr9_EoH-Hn z#=CxK7v4_)f!*4qeno!0&OWyvTUt|4Y|I6D!M9*V){1qAy->{ItT!kW;W5L8N86=< z+Tmb{eql-AqTHU(o$v?26XZ%^^6mwr!Em~lC~#UfN66Z3mR!RHC!%6bm9T}nS^ksV zM4L4Yo=czhCaT9Pl+K2{!VRi>pIv-zzuHbRl=ov+@W(om4M#tp0p9T*b~#M>z1b z%Tf3fvJVS2cllv;6YSS>FPq+#Xk19Ln9oLZG4Jyo`LGP8UE|LvDpZ+pVc}Iqd||;C z})dK^-9^%%4daFgMocT)XY>s{_G`nvP|zbpze;pWa|EC2xdkpE>-{C}Pd z{eRhW(>zvAhwTZwk5tN)6;&iGbY6HFHCZl_saKfW8m*GaX0@y-jsqkl1~N#R@k?v( z&#T)26beLmj*cDQjh78w_vTMs*n6jayrvd*Ln?&EKTf|#&zrt+rw!7=yDT2nb@r7#?SnnQQJO+0{zUqp!xJJwI~{1~g&ZAE|WxC6JJhFsCcQ6SiGScQ1Z zoO)qfe|oz*`U#5?D;(jpZu16It?3^Qx<>#{UQugAFz~9w*Au% z68 zb$EE0{Zp3rGqoDx0%~I{>|sUtIAT<>rdbLzyGs4#!*=G9vhCzD?U&4mpQeon1!t=h znx;wwPjEV8iUl&64IA-R<(B$=LJpKfNi&j-3ZJjwPlE=1i8-92*uM#(#ZJ(&_B`9m zKxz-9ecazk0UN`IA9k-to%$MhjI3L03{*vxq6Jils`W@=kFAEi(~r$VFRla~=13YQ zi6#={pmj$DnFxZ_1d^zMQ8$2HEL9=gG4Wxx%4i0ub-1q)%&ha^H6rXydfZ8NsI4Nx zaVTYwqz=nIgYHP1CRL0_RW(Sve+Xp3rCz5rkmP_DQ?@gd4Y({RbN;hVN;P4-S3wPO zV_OMw^FIDJ-o;*f3hXV^0%ia)m-nCfpHVkkYYUvrQjvy3gHkQH^>0 z%bN&!uj!U5*J(}F4i=nb@6BhiAjKarHmUu;)hd=ag+Dcmwvbi+_mIkQ96OYX+7I2T)#7X{L}!VBaaF z$vL}@O24=-h0ZI*q**GVs0WyJx$+{xcfTPP05<+4Z@XTN2$^)ji4yc?W&rcb0e!}H zw3bBb#nluS#b9&N1!6bUu3`_cTO&zkDy><2w_I^RAb>XXIv}?JAd+;-2NVlV6n1Q| zMY)75P{t1^+D)#jX)_E<1ep%ztdQle7E|HnR9{4)VZ|Fb`23@7k#e?))4NV(7?5+A z6Bme9me%w<=(nsYoTMfGQma2E9aRe3;^i~v6~a7D7z>g~4p!OrV2Ua`&mr2wcyGq` z;Zi?Eb}*{1S1lGh#Z~Y^Rr`GBmGvmPe(KY$JwBS70eFAyd?#eJnz2r-uD=Rsrp@$m ziMS4qQL!{~Em{xK>SRj&^#n|;5L=ZuY`UyDv$5rnBFQjS0-f&59$y#A{bs{!fGZ3< z1@bQe@qDZ4r^02f%3{D}d|3sHb%_B&VXNAGWB{Ert;vLjE=;780xgIfaz1XL$6e{& zW3*Zb$8&(@Aj0g3bzp5i;9N@0^?0zEA`h%&q-K>1JumJB)BQAs!_&5+zFD)H&>IKD zI~K;LgHG)5$rmI#5V$%}T09f3KirHj$cARW1nFL804$tfmpZjDHN9#j`DiBNNMqDk z%f{HxS19}S?jd4M8KZCdeQ{(ApM}CysF=c0!u+-W-WD0qn4!go^L=;##hhy;ulp2ut4m>$hd>1y`CkXc_EMwLb<#D5&^LTL_Czz-C9sYl@o3}gW z$7sO@_S1h^9TW+ zL;^Qj&0{f}Q=Cm4O**zruCBNCDkM^@1Cnxy`83^?7G-u@PiFXGHR}5 zc2eFW+MTW;=L0#j1zk}XrtZ;roh6_>=23lOR1R;AUnOyKN2|-bx?J_!l^FDCCh6w9 zhNmu3=6{yF&#jzg!SFL%L6*{W@Kvk$COdZ1zmsiXIvzN z7+`kHrI!5Ew_G5)0})G+?v|*IkvJ|Bu@+PCi9GpqB3Hdp-RRR6muwBSJp6%?CQ2YT z`exCP)&XVg=c;riU`btap{1<;FCmpSpivOL!CsaQai~U`3AUOc;77`F?i6J_v2QrP z!pm%Q#u-iWlqY{l2~Fn~C3$j-lg2Tm5}XC_7%~l7`;g`8v-5-Wl+vBat;xW=HEBWS zPMT*`K!QsGc*jnZkl_qf{2R~}Y$|+nIR}vrD zm0E?XV_9zu0%6@-Xc@x9{x3C_33|7zQlQ$5A|s4%wt-4ABQa|OBTm=pd72~%_x-iX z>5c#_1Hfo;ozmnT&S9=;oBA2(9dZS>9TF?=;|Fd4+VkI2AQA6X^m zy@*Ob6B|coeqn#H5=h_h(xpN4o!xVr*&~Iz2;dElnxy6vhiRk1vOY&%C>7o%r8FFq z1_}^u9N3 zR%0Y{BzIT*ZUpHldNXxHRy{Ai3eZG3W~>U)yYH&|w;6Lke2~DVsg-*-xPtXlZZg?x zKK>(sA%R`o7gQ@`G;{Cz!CjvCkNX&+wEBI2lHNgr{-k!`XbjIJCU80@lu#OO8D-Jp zgLVI#6?X>uA-AFf0K;YGD|x27o7RwFEATs+=YvxDXvJlwZZyP7=7SK*XKe*cTw~n6 z{I`eE`sdMH!0Yq)C zwqBi)_xKWZE!?2nRC^<#FCtlyKhbbxg@4sC>$<%nNo}iyt+cSoPBU)wuhtsnQS5Pm zj5jWZg= z3_*pX?(sCA2(mJjOqwCYzMw|ysqGq|p=)f=;3V^C z*Qc+FZ73Sdpz0Z|1apF$aUWN&S;5^yIt>YJ^1mxqk~4Ooh6$+bi5#(`TC#?ZEGa2( zqs!5YhF(yvVdm`dA|6KJ*_kFl(qQnv(DE4hjkamLW7LMRn%gJrUc3USVa>DNdZkK~`%k$Ql;B&?!vP#7Yx zG;l+&0|j*m+nxVAYB!{4ui+2jCjzh8w zVh3cUGUBuFmiqqVQoUEn09UjheR}6~kOv%KchkU;bJ6;9CR2&=iGP#z!e|pQ>OrFY zjHJzjH^PH)FeCh``=rv9fL9jJ_ygP5e?a@EZ5JdcWt%!0{g%>6Atemhp~a2>6=$Ap z`(O@2E6nMzPTd=(THV`(643K;*F%Yknt%iSIJt@tm6tl?jz+ziX!rN12;M&0rt_9s zrHN)OMHm_ztjwSW7ZjHI*hUZx7xUsRjARxGI^S1*s(dDBlp1w~iKkvoosM<_P!-as zXzIhvT{D0^6zhqPYXZaO)HF0CzhjBV)pZX#o**>|LN}rjgh{fmp4dB$a=yiWRkbKZ zJ(;ON+rXKh$a*!TS!M|N11VGmli+Oimbe$O;XI0;v9#{TNaN@9uq>{QxA*q@zUwdY zd(qq5k6h~!x#~4Ai3$1~F;!Q-ti?L0^W-FNv6i800S{vn^hHvp&YGyLAQ)>RGEip< zF56SX!eW34xzn|HHt`I@l$9x@ksfUKpFDGA^p{TwEHg1_yT{U*lfku#imYk`sJa0N zq54o8$%H#6JM2SNfFhbkg})CnCnH7Bw7!dTv4h3~bd6rQB%JUSv>cvsTl<*?vn34Q z0T?Yo_O$!pz_MwjT1HZ<~fJCX`<=U)<^5rKThpdTQ`)DEoa&R4+$(8=)9txS(X+$p_=5Iy>r;=-ha_q_D zMb{z)q*F4s{deWgT0^%udd#Uhuv2$vkm-2)%1+i3mz3p*jlp5*Z>EV|%@DikRD1co zh}9ei`sACat};g-xuZ_lWrpAII*#%_ZZrZt9qJWbQkO_cjSJC-$FdXnSlSiV0AXJ7 zv^7@;A@#TA!Lm7YINFAiyVn|6 z`w-zsaaEK?vTX-5;8)bH{bc-F2?_tGJ&jMHZXxs>{06;%a@V05wtn3sX*SDxqTnS^5MnbSsw>S56s@-(l z_-3WEKwE_Dw5QX$$^>aDvY4ZSyFPDZP)b+MI(-GeqcD5XL8V0xowf{FoFveY4hVM0`vqWfiM zZxPZidTRBv)`j9D!Q$jFcwrm*Cn<>HH60aW+s@X{FIiU|;(6K1-sc|W<4eXE zU?4rUdc>L|_`?Cr?3cnL%>Ti%t-?_%>Re8 zcZ$+*o=Id9bNVok4 z>FV@Ga=NvedQx$n_SK>wvqmkN=nzxSZJMX}BsxLbFyQQ68UTNs>4WlUXrTpuv#%Zy zTcjN2C@fPFwoO$JXP(pV6i@u2=Q1p>AwJjETmb<^xqhWkNuw!T1P?lJzmo!c{^Ra` zKs%owVKwfzc+HzX=JYr*K3xH}4-U8iM$xIj2ErXF6{qKbxVwwdqR_WI^;G!-JZP>U zrK*dzeI7=!mKV#2kFl-=Y06*S$-Pt~%X4ICTHFfM#VcWiNQVz+Tks$Z^lVghYJgJ* z2bSMhSB#ZfrEBl9TY*7z_?+hW_B?%SV`q%}-%RB}%anNX!y)-Mn0M$j**h&R$K5TV zRbl)?CYgDwBdUR%vLaMv_cTb68!~le)5aF@+^+L$baYPc7h{8Talqko+yMe^Du4;# z-%t3nfUnp9rgpg6r03#Qqn#OAy*uC)<#yO`yP6zj+8kZDEIAo-i&_gu*R4Bi-y&K^ zvSycz^>vb#y#=-}PITPw(&49a8uxYs=g&vhy5+L9x*u9mt6b@5Sd92S{;(;yF3Y%9 zMhPs%LQikC6-XT+IO-%>dRWn6#NR`EWV)?jG$Qg6cT9;NV|3tzvn8J>2GJX&m-eCK zXd6(Y;9SbrpVBNDXW<7cGY>kN%-f*WKr?()Nc2ILOJYs&7io%2b(ia=W02b+r0H?F z-46Rl$gcTe7`3YZz?fm+YIqbieh%_zv|`2=C~XV3CcfRzq`8u zbk5^cAl6?#I_Y~pT-l#o{OZ@2R;%EwOVA%~`gJV+5WJ&f6#E5J2=IC{6#S#G#cR8z za|=I;XRk|t%I(O|5|@(V^%ptpLI5N^`DI~#P7|DDM{QA0H#my9H5y!YF52Rvyk)oJ z6;{D5q^jq&hBNe{P2z^*Ikv$R7}>dfme!1=+DE-Bxl8ves?{h*%g{RL4|v$?DsXL0 z@uU?=P|&&OH12n{JHv&_u0ZZogbl~VFpBK7#g`R zsbu;=vC4!f_}z^yTru%FF0q85@rfDJ5SI`#EU-e1yIH=y72?t(F7_Zh3yV9!WkNU4 zkb$b>v*S1x>Pfx8bysJu1R)i-rDjjKO50^zyQPx1&j#CN?Y~w4clIx+JMPr5}cNVQCR0_B`yg);ZrLDpk6N&@uxvc2uX{?xc< zUAaDA<~IQ2oDPC#rlPrIRe~R`1XR(SvogJ~dOOX{;U$+_^`1~ujIUE%p8h(=brPMv z%Gy9I_N`hVuy;U8b9rIPWPAQ9=pWOm28lMlNKGR#*B09zVn1D$3bWN#2rT&w5@WEr zXi;dJU`*5*KP-+4SkX`EV;Gt$yCUqBL#A>KlDY6MpWtHT9GvL5zjjW{^|z&5$Od_q z`^5quvRw#+kzC~VK*O!6K$*jv{stv>F>oI@o7BE%Z_c*5ouiXqfL62jyP`|#7RQRg zQ8as}4{xVG_eRR5Ii8!_kC^d9l0|H10a41(w?wDhwh)W(O6&7W)5(&PdG}pn{b}mD z_foq>{HmnWMb(54PG|b68*IKrV|y*Iap1=iw_P$ojFSB&SdmOuDD2>7yB{#x;j76e zw(gTnA@o%Ofp7qGH@N~~4#wxb;#KV4@u4gne7*KE!DiJpP4RLvLHftkTjek2|o{ zoNDXSAG40EJ;!Xgt$X{LzWhh{@tQxVO;f;Dy56PE^_f81oq>d4G>T^Mf`T=f&Oc?X zKnmw|Mb_Nq86fJ{w6E#xzFOI+wg^aY-H&anrYo^+Ii*G{v^`tzdmrW+K^Jk=a9Wvk zzB0r7b%w(Xcf&3TT8$gWTp`t#a~7QDE+yy)=ZA9rp`#0{N4Zq-!Zc2;@BuCQE<3Y= z+HSaBAZ8(AY;X$A`AMnnpqy=I?!`2M4YxEQiLs>|C>>^QHXYIb+?WCByK2W1B=LOp zMLTXgxe*tplp~7f9PS0*w~&pQVy+(Xx6GqTDW;{CR~x%rtJksuGt5rCN(7*5fKN#$ zIZ-Ycp)Am=)gLx2zFL+>B55)-Gz_wrfu6`u3w-{GZTqdLQEsCSBVSNL2vsado)+~< zj)Ky-1)-6rTwX};o43hgr>`r$t%B{jb{TYi;&Y8E9*=(%aQYtf8MAGlrm7$)%qJ!= z-P6X(3T#I+o_J4AFRR<>`SN*iwp5MVw?64Q1NpFvc#Hy#@aKCr<9K>J8jnl&Vs-*yS`v+K^3IxPOy3lACGDThx_0kHLBO#*Z(bG}H%0|j zYn^c&b}bwJS&n+*`02^3!gc@Lo~(Cn{X>nowI-T2+eue}{3U{2f#;fI1((N3chRLy zHXkYbFVBeVdJ2dkr=w(iw_bc_d=_7tD}!`nLX2m9l=3O-Zhpqn;93matDXkA8fHrd z+)zvsV5ycXel-UQVEq%7E9nx}Xs|iOTwYsuISSF$%oYdi0vlonBq?t&6JfMv~9qVEne>_g>4&s?|O28pahiq)$PAHAXy&?c1P1FMZ5QYe&w| z^VnoS)#T<2E8^Hkasi zgV1z}>+`rK_}md_(L%KY9 zPcb$ZU7jqg*DZA7AoVWiTb~9L>yuIriBsqjfIKXAz5VPP+Z;LZe4ZNNdcL9O>$v*( zK*-+YusP|tfaC3&2@Lf3t~7lC{d=>nu5aJA`*)LW{3>zi{-Zm&zP5UJ zLY_jBmTGc*s$Q9Co>@*od_r0!#D(V_7fd7lyNy&oC{B|BNFQ3{}P5=wG0)r!D<3L0q& zfOW%6C^qOHar_CgzmehybcnoJem7J%0RsTw|5w1@J@s~G=7#!KKfl?>98B~L|1n|} zov3U3%~v$A^?_2fau#Aj@5v8CWn&}*p~sY)5~I_A`KQ7cX6?6bk9ijG9<;QLFAopT z({5B5QIiV+$$?cEn5(xD;23+p6|=BLr7iyS@C8RKP^xCe4!0guKbGnCBh6TN1t^_f z7+Px>hbka-iyTsm_fX8LpIkW|QEzXB=sGgCHh`kDI?K#+ z51pTaY%oW-S|ragNCeCiOvXsZb#S>E5!4Kjqg$9)*4@Y{fQ1peSw2ES4MLP*0ZbK~ z>Ehi%=)ZIYiY?g`b@WDOtC8Z`j0SsO{P|y%ao>7;ko6<&2zcVHO9Q0L0ntm0vB-41 zV6{xWHkV`W-$xmWm61~Ya#o3+ z4znSc^U<40(y-V*-oCir&X0Q7A9okl-0);_;EOM0mWJl?35q2$AY_?VyVh#gd^-L{ z-m|r98Fuo$KOEnCh4Wwc$=cS)*h<*k(CM46N8iEyuR5!A+}L-|JA%m53lyWLdIEkO zj+H6;ae>^biMA1s0rD7Y$~vpB?hkWGHem2WJh~=cuU5p4%x_+qc>qZY0_IY6__-GU zxids)_CIZ}H{xHujQp!=Ezvta%Obfm!QR=;_{VT%&$h9Th14E{+!nCOk;#R=7^~59 zOI8p~DPLmZj*{vmItQrDGN8;{Ar|OW@uP%xT7~a$0lw*?uo+S;6H#%|R`=OWaR&tV z(bnpyn@g-FqdR{V9O^@vG$^Fw&*6WKx(z0wY3<5LU>TP>Sy2qEM|wMLH}l7*?8se>feBTzJ>Bz z{?|Cye}EGH9$&Gsb#yW}RCKnpb1-&v{O54j|1q)=DXFP3db$xQI67Ji3JF@eN&33~ zim$hre}s;2N|Xs@&=WqB}a;7fTzHxbe%U`5O{|%p%t*N=8fQ`PDyN9vE|GnaF zMr5a?T*f8GODiO3q5iw}dWouyQgclU@d4&5VwKtch7Kgk5=QPDmEpG#{O{1o*g6?I z{y(4^qyI?@_utV7BB}msFR2#Af80atscTd`evY_j5(1PCcjdjU(<`=)dK^?VGBxfui9z)tsZQsI9ZjH~rlI z)47L~m4=d;q!*`+aUG+dko-vlNCp4j)z(`n^{*plmc+|j|2MTodze3UeM2Puc3uBw z0*+4lPR1rywyyO59OtsuH#9T1`A3}j*X&O1UmR@0z9QQ?W0wdZ!3S2LoJN!h90W## zNHpOAB`gzLVwx6C`-o3?Hr(%nc|Ij!M@D+b9&$-EQ@pfb-@`jiJQSsACNNpU-GLzU z`dD3*de*r1ypa}hB;P$RFG7~JF+>tW=?ZUJEy5KSG?9~Ls#@Y*PaiU=T`HXEd&YfqTv$?G|tU(ZG7=^`Fq7*{<)A@brB(QIe!|vx?7Id z{rIri3nUMNgbIWfK+6R3^T+m}aBD{(dbL|c-F%XDay`(4iW?F(%J&8G;{`6R_eU4( z&Jo;#0mcWN?$JBs0!$_Y_T+8`S(4vYFi}r60sUo);3@3(jw&Z7+G z&;+j3^5Cg{0@-uKxJn?ACWt(&p?PlR=c-ZRNFt*JL)bJy6QI&I0)qO9aA2c2=5ryz zH+KM@Pd_%K2tw=KxakxkEJbfgUc{ZFjq2`YPGleHxZ+jRmytX1y-D zZQ$C(&NS8dWGsc_2ie}~2}ZW%z7X>&4I$d=Xgsqp4%)L>{X=}@@ibu)VU{NN`g1!E z&|{BuPvRrft6i`ce4k!5KZPb~O~nkVVQ3u8;ra6<(kt3PE#GiT&HYm3kAA>=%ZHAO z0Kt?*&lc#_vJ${_SA`;|S$KVZ+WUCJ0;P#WGw9j@A(Yp}cc$E~1T7enDV+#U0fL@w z4;wi26rP%Bs&rKbo&?!??P<{3|*5tXzQ8Siem9&NQ}q5s9o3~%oiqf9<6^Zt}0ccts%j9fBr`wzc|VS$VGIqpN6v%_NZH1AZpYJFn+rZRggd`JGrr{v2ck)MbkH0NV9c_CWIE<*2O9 zz9NzOg+g0iWqF~K)P<|UoT{L>j9+CyNAp9H%{ySDY|M$DkZgLx)ACjj@r{~R{;UM| z@^|I+sr-pWI&-mSkP61(PsGPhMvsM6;*=Vay49hS+Xx-`CS%kGWG&|Ni}i3+g6&%@=b96>~`6D|`6 z^PTEyk7jO~;^yXZ?^vFL6 z&DeiP8=A%Xu%huzNH3;mwD7~*-?(azLYcIpESORAIt(ZeHHR*$SWhT57oHyoL2M`(*)R&Y+C(({4Q2 z3ea@r6z1*K`B1-8cF;8yN~OQ}ZE$(iJfkn}dRDt3SI#_wACVD(j+WZ#m*>HFXzn(A zYf9jQT!u?c$Y%aJFI>?ekrvtMWg{EIa{CEdxhN+*ee0y%Yo~n-O_`BpdVolmqF%{U z!n_slf$zkS%DDTjr3YoqAp@T)%D%-(UQGhGC$COU47u8$FZ|4XP@-7U9yG)qCM)NR zzg^XzxLDxNzBLy0?d_@lO>^#!PR7=b|B7~w|M;cL#PpBe$bXJ5&wmFz0tB#aRe|## z%g%P#10yP>mdTT-b=9nOfRV)JACEkxk~9S}sKk7`Ul=m3?A~kX+Jt#GRR_j|9KcI2 zo6Y$LS4XL#H-@{)zBnN?t7{5$Y|M3 zK+zrJgVIGQ4&@PBoOGYluFhpwr&_HErBz0(+8Z;)8>b5^akC$W*1DRrLzRUvHq|2xH)O;&x4?8TP9bNa3J- zUhGHP+yIw%e6}h#=Za|u@)_;y{6mX9G*X8rL=q|VaEk*I@vhfLiS#+T>qjEZoG%8f zo}WICO?s%E>~5KpfdSs9+xz%pn>$fok=CIf!bu){Jif1;3$Gj6!y3gupI$}OyxJg- zEH~%OTcLjEN&e$bF+0!Q5dUgh63KbfoS zM-h#>mHsr}m?Rb;YM>p=JHnYPv98C*Q~vXHqumIx(j8;>wK-t@O8r&&Q*ZB4-J$7F z&g^T^JZWp7=cBC+YScNdl-CE_ME-_MQq@^@ZN*C?;YU0-$#P+T<|H0r77sY?Bt-2C zJicD6+?(q`D4QX1sm}$>mF?NWkD(x=HPJu|&hpPjZ=Oo7wP;rwH}aJ7y$$SrT=%Fa zVyPFHmv>do2mcJW3If-nWsg{Ct8gWcf#u z97)_Tp5PyNdRNY^8o@si={S3zmq(mQS|(%mTdU}YA83d7@hI7!t4W`TqWW3_HeOq` zX1yw@dr&k;Bc&6q92b6v$p&ek^7=b%F)n?$o&~_Z6KRkMj5AonUBTnSUD*`&?XuDY z^tExX#(IBH(dAfoYM@P_khw@LVO+lYgjPYWKlH(AD>hpW-oa(B*&Q8#Tuyf|`|&L4 z!jj0%xO0Zqs zCWTsO!~k^)q)Uj?yWyGoW#WmTRsi1ivhwQg{nT)0L_WO+ft65$^v7}`Pa7tncoD%Y z$VDic)ItPcA%E50vdkMu;R3#((M3p_#6rYwW+!95L>pXKdWOKF9ovFRnwx}B5b`Bf z0@4~fsFM8c_Q;F8y|Ev<)=P=Zn_cU&SRbR3NBEY`?-xzE;AEqcTgf7SY`2M_9F+?) z6y-=6h^Sb?hDHJp^kSEOc`dRD+4MtJ1Q_s=fu|>E&=@4|_?NC*l|NX>8axv!_0;;o7 zoj-0XBxZe*Fw#zCxi;SX>^y?;Ml3Pt#Ao0c{7C;t)$NPOQ{<7pMs?olw2gPz9F4}* zz*S7UBLvxhy$YA(9emdq8I4KM{YOWnM>~0a!*X&!J;`_k#d8}D-#$mgO)3x_L^oWT z`aHOhY32umU86h)xgz^>{vm)sj@bmDxgEe^U@h-8Z`&3!_!E__1r#`+1l;z$#R#B7 z6Al5CLmvG86y9ThPvGbW^X|5F8cn3;2td{vk#U$tEWZ9Q0)e;1+4APpwc%WkZbfyu zwcEq-l&(&v_lw1&N%r`dz+y7uMTQx?tys@N6}Z^>At6SY7g9x7OxM9dHrt)>`DJtX zoNg?AJ;8|KP|7jz%fDk+4^E6n*$V_3}@7nX6gpnz#Gjw zN^qA?CdiU~Q~8Fr$obQ z?7&(}(Q|$gKaE!cXpNv;o!_qoa7hsHTz0O;X&?4yNfWEbBT8nThE`=?C1Tfm%1Xve zILezBrs0O%*6k|kV51~{y31a7CuHTfZu$rsiIlaC`5_M51pGTFkCj%OxwFiJC0$Tg z^Uw1K&at#oBS$Y}+9mZBG3O(@K_}ye@fgh~y53#Og@@KQ7U+SY@-HuRCJxBmm4)B9Y-S6?;Gum#UpGBMKnw7&C{7ldT7zx+yL*prYA5GWmHQTS(R5PdK&=qsHz6?5@EwGh9Ex=^$GRY8NcXkaO!8!Bz$Qg7Y_ET|SXU!$}kLp)n!ya9-pi8T9_E6k2oaY5k zvfTrBOwwGE5OcwjG_)C-JPk`qIxFgj<*Y4&!sD(~caTvnh3 z+FQzoV8t+fL5?NjHXY%LVA))@(S@!iyrt^_U`qL5vo{AR(&v$|am)T%3I^p86}jHC zUqP}|AXoA&MM(qRXtmr9=W8+6u^Ral%v8K&$IFehspppYWd+^oJszO?;Hj0a*5%jW znhm_3Hrv{JUO}%*A#=m~h-7TWxoiHus z1ygIta=3(Z^;NOe^Dq^5e$8F&Y51n1mEKpBE6kAZ1X?41O&^*@T(r(ws)pNw6n~)p z!i?Er+V*jT%zFg9^O0P%u!M(`NwN3fsp{>C^=%A&sb!Xs%DgdmYShv;5=MR41Px+p#Ow&tAo8MrbWzPxQI z0Tt1_#;baM@!C(qkGSQ)PwIZ)J@6EAseXEpc=NK{BBI@g$ln_WCUzQ~8~3`Z)BHT_ z#pXgi+_rYg23_f3D712ktu0HJclHK{L|QLt`b}AOmesLy>fs9o68v&5TOZZ+yMWsq zk3_P#>VZ@}d=70Vl*)ddn(f|EpurueA8?A8J;LLzT<>#?NpfkVz)2HMX4lMJvpr%d zf(PyWNb_a0n5bjT8f_}@9i8K6uOqK|%~u3yM~0uePUoQz&FEC_BJ9AvHJ7|J-fx%D z{R7&M+6HEa-LqIBy$|{MIQ-1d{gBtbXo$aCP#jLt*sT<{A|w%|d+AJsNH(wv!73gmt)aD(EbyMWqCXqt=WGhSlNk|PFT#aB zC{#(Q>zMI|-J>@>J}5ozu_~3VcF}l09JRH}BUFcW+8c#G%uOo-<-Csp=|b?QUo8EO z1}P^$6WbpXVLVmJJQF3BoWOo4s^$OMyzKY9tO00tG5u`oC~MBEjJ*#YJT%|NPT0ML+ne~c+Q&_f#yctsbg zQIp_nvGk5l`4q<{_QPglUQ}Sio|0$9e%1z4{4Jh`g>KELv6axpRw?cdC?+BuKX65% z*eSEnAu37DnNtoTo(BT@ry4{JElTDZ+Cr`Lj<@*=lB^>+xAa5rPy~pp+U~d)PX%`| zZOvNSGl81BR9g#@LDxl)Q;y8NEdEI0FXDs*{RNTt``&8_W7#xaNpcyKx4!1MKa``u zDbiYdi)FNQ*^lx8VD|0`Z%O2NnKgrzlA3B|t^!ImVUXIROvcEm>cSzk3urx1sf&AN zyf#@qTR9lYM(zE3A<`~^A%y~X<5rEQ3!J^|aBJtc6dZb5{~XDimkU%aL%QiSG-WU@{mM6?Ry{<{nv zSwO+sOc_86=rSz)*dnI@QWUsMV8w!L84R$6P5>okELK2D4-+h~d4v9J0L~7FLj1~M zGRS56G_U~5$(Yi9Wja#R0GyNKP?lh#?`6Q$#PM((LZk{y04F?AD5KI*0-SkIIpLdq z;`v!i7)8KkSDB@}QOY+o#1+7+^yR2~cQyp;Cuj7ShJe%TN7@+x80YuN5mB{3!$zg2 zKBe;T=lc&5_^3Zs`x~OTv9YU2i9UfG`y@~-NK>3Hkxr8%R`C+MLL*RnS??i+woGfM zANk27w^sCJ?&FU}uYuQ0<_)CgfthIMUkP3cK0B~qFbua5lL~J3W93`BC_2r=7sm({ z!46g-u_mGQ>R=rgs!_^|CLE;EPcSeLjjr-E;01^+tlvp%9>S8YpsZqsjTmh^o~zl4 zOO>S!qQyr!nW7KiCgQ^@bpiJL+r8bmi4{PduoK{A;=|$FfWSZoKx(j}#ev_}cH?*l z2*8X|o*bRs)i<&C)sT(kxDk*(0YJ(&=OO$EtUbDZM*e(X^7t}>Kc3$u5imSuT@&n| z7S0UXTLPP2sJfyosPsKl3zD zyn|dv1QFSKl6F?QnQ-=QSDb_Tp`#5!bDdGa7*XAKwxjHQ5rL?qP4K-kDs;v5_!{<` z4AY{&)lQ9ol`$lP{_@N)C&Q+M@yx)$f2E(3q6@Yr*3DuzU$g=`3e=#U3Pi!%GTJR` zF|>R8ri7sS6=v&k(ke!mp>v0479o8}yi0piX5% zegjn>)Fj+Jf!ocU^ z*Z3Qd1+)4@ySF=Rr4vjoxbmb(R8IWv8*F&z?IA3*Xik@U_NYNH6wH{^E*ci#r$Xgf(@5vHZzpmLG&j+&AK|+#L-x2&B`e;Y(Mli&K zv(^`EkmsN8cLbWEKQCUDzEoMhcqa-%SttXZjd5s^KX2m$6FI&FokS9DhwWi5@Eo}7N6ag_plRUPx@VZ zL{uk<$Kxs`Qs7&bZy8}^e)`SCb2DZDUa$t^k!FK2lz*0JeKRKMtZqDUwSY8Q$f%_U zuaf#1zIQ$OAeS{vyue}s_<;cKbAP$*j#-UEmAVO1`$Tup#WF3IBRs|~;E>M{uB7fj zwwGT4C{)aa@iN5zFt)#-BOA}6H-j~u0ylcEzOG*9hoVf|=Xy8nm4Kba$>8Vi5)6RJ z$u;4lV?6}j=#xz=KnbXbI&v82C8A1_mzW!qFW$O{?EiG?IH~y_+qi5xA~nUJVX*&P z9gmGb0Nw&ojD8GDzH0K_mOwK-}ML(GtCHdO|x=&(_`ZP61f$Xg2q)9zmnb}Y)=|25l`)#tEGP7|@`&eSL{($iM&JW5nH8 zyqkqn2z$fWXAkcnH(eT^T%3ICh=xGoBHG$}WOl(dG|Q=8+*QG**1Lk)U$-XuQpMdY z{uXjmMURHFDTH-+ZM@Ezv?Cxkr5BFtKo@r^&`tL);7w63ZIPN9Z5xEd-kh2AwUIOU zM)O6$DvWkv*k8Ts>AIUVxAiWV)zUVCwKuCRS3m~S^RE7+;+ti%7quJFunuFak8{S` zRVHJwL+jFTBe}2aj*JPHk0yUW;aB9wB4?(bbfU;YjZVdC>b#B+%TECddD)zg;x>h+ zf zR?EAy*t`D;=~h-Pzj2rpAl8}mNm;Vc*gl<50zSm)679}9z!^ zi1dF5>29wsz+ygHW-$OWFqw`^|Eg3gJQTJT?pUL#=$qJ03~@||G#kt@`?yk5e84Uo z>~oLBQVw;!VqB=rR1S57i8MRRIbGf=m3b)qGuYQ6{{68&w^F97m+Ls?>-Fnz+ytOH zL*ETfAjrUU4F@s}mFkMp8O;{GuXQH)6u8L{F#ee6 zuivR{zwBSc!?)8Wbf%MEu7?gXTy5ipa&F+}Xx(mfWv0mT0JZRJ0`E-dj2?I#y9c{w zhiI5UGrQslCh)!4=>9Ek(BAOz%DcPfM$|wbCB^yF0fpsVrhB2jQQCf0j@yQL=MY8DFszONJ&7>gEyD0*!jc|vyu!mYH9y`(m#6u5r4ZAfQO8DA z6f4^{V~>168JNQJ^BKMQpYO)?wUn;x8gt_F@S1~pFnc7z=%iaC(5=gFI9J&&Z%eBm zONj7t(`%^1U~qOePVDp%fRht&M{5@VM=EW1c7B#^8xMsy;a7{MK6^tCcbh-T(XE^I zvAiYl%4C>h^k>Pr%)RSgch;SA;xK7SZz)q)UX&7QipGd(FzdJn6DY^FhQmZmFD$mH zG0(s!&*WSts8EjT_k}G*JRN37_l02^m^e_7iB$ZFFLPscS_TpCy{7eo$sgtYDWk=Jk)uC)Vsq(yC14!rV$ zZ{(A2E|>|N&k;H)XvvcgZ=ssFgf%?Y!bVfQncAcdm)V~6J{y_pSgNK|NN-!;%j%x( z-=6i>f1APM-siG-V zkJ0*M)Q2N>m6UJCnP5bP%PV8zJ894G;zauxr~!P5H>X{bt-+T>WBoUNEmv5q-*nua zDQ(HG@2l3dF04(g%YR0-ecw7gZPSL~moej>M}rz8ao66aZ@ztO`Dp7VXXi@~$=6j(Ju_ZRWxphXwC+J>$&-Z_g)(?bOYP7_X3-< z{ppu&Tc!i(%^BrU6C#Hei#PdtF9_tCg&hBn;O^UA&20 zXbf5A0nxYbij{WcTIU?JY2i^*#}>#W4RdU_qfapKVU+lROESX#r%8u=(2LZY=Rzt^Ee8HpUg+Q zjY@N3p7UaE%T9t}nPx#va;wp(Y~E4?)E`a?PW~ zq7oN}iDS|Ud5i-CM*E3F%1Zt9aZZ-6e%dY zHZPxsY>M488$eW7#>ROIBIe{K+dRXeEwvYFIg_=S$aoAV|#<2gNMEO)x` zV{31wk8g&sKouH=xz={LI5r87|^%B%> z5-GbU{7QT263vhNRLJnHh66>E48Keoa!^p`kW3gL+V*s5zY`DsbF*K*1f*4MPto}% z?gjP~72Q@DwtbuEQ(=KQ-Nnyl>Z+6WBclkUW-FT6e)PDwbe*}?i?P&^zP#dM)sc$GoS^Ce6Zh>nV1G38N{fWbfFxE2LjT z%;wwoN%4GyL1PXfeI-NR88B#N@%5=0Z7Z3$J~nHe=m=D6EBzv_De{ln?`NoabV+f( z*2^PR(4;LT z>n$djSx?fQfEGAewS|tCcj5ER*6x(D*E*cFC~LiEb*nJx?nqwSr>K+MmCkP=`}NFV zH9lW|nDdmde-pi*%AX0Co|QP0P&Ba%VejVex1lhmcAViNiB6Gxs{ah$;_F{ov8ZAD zVKjhJUou9V-qbXM6aMqHe@fG(%~D5q;tcbvj~dff+aSEy15uLG207Ea%g)un#hp~* zc!2t(X~ObhUwYvW(!)bRveQ~|gXgTvkq7$H2C^!vl}7WVOfzg$Qz?dQ@uEvjn@Wbk z=ip=jZEsMW!Jhox(c9miJq|;sicieS#(emM>I!o1s{s?`voBVz8g2&pSMp}F0?lehcbBA7Uzh}H}V>EPjZ*2VJW{dJrnjsChm%x>+0?bnDihJ8k0 zi?up7+j-pzMPKn`HnFr^j{=dtN-c@hzD~ z9HvOnaIi&$=6*u(BW>KtG1IC0RlchtHnvruw)fR_8LBH4pgqhzz_%G8wIon(!;D-b zo5M9sFW86rA90LK?+}hibcQcUFbBpElQMYv#5L<0=8cb@wSy zs^7hE?b>6Sxtcd{+n2}jcCvBLt?*r%xZLW*%;s@l5pBXZO8w3#Dyh2|$_>PDnVhtW zoJycp1TAV>%T`vj+BA$EL}21L(14e?@r09L`vEcz8Hr2(oYRjnPRN^=F3ieKrf(rW z$<~%NsiZ9|wp(+ic=~T}Hu6ZGEXM~b{5gBm=5qBsYgRJ2EAC1`3o^EUcF#?w7-RUF zL@XJcHv4mHzhV9nL;XlrV$r}W%%w>?o41~TV*BmO97l*LJ@IiBdZt*;pa&OPeZ^<% zoeirUok-_?%rT`m5o5mk)Ljr+?U~KnVbj%~uFBRmI!Tn-Z2+mNVv<(_R6M1cMduG4O7P5_R(^~syPW#%y z;lt9SktLI<7?WzM6=&5~(%0WwXfPav9f-d#ZOkzLLoi_bSC&EZ`w97PLwKVsP%6vY zLpl-^Y*AV>BQ2^D8Ep+!_GBoPEF@!;IFIpp!zBl))6+7;50mopn!K`o%L48jQ^ql` zCYcb-h;o`|bc!Yc^oB_s5I;IEy*NqT`=%#OVML0RWwB6`j!`4o)fxyI;`DO9-Yc<{ue^I{B6)KY#&Q!l&ut|QF0t#e zEDROzorAy_;suXHP8}Jy-zuBG(iZeYrtpd6#q&z1uo%Ct_OS@G9?FsvZ#ccRs>$2) z6{Xx7KXFh{=Kp(cAQi{)PbNpofpcY*ytdQEWZ`>b;M~9609=cD9 zuc-He(>x>eB!g+vvB%nV2ie*~%K|V}Kf+UP_s=mynjJ@JwTbOhN&9$>F-5AQXvo6=(D&*-*X1n7QAH`0GUrI%wob5Y}tgzcDTZw5l z4k>IkUc6jiuz+N4*Jo>M*Z(nv4LE_-?w>JnXJEvdIUzKA3a*=c{bC1edpSmcjU<#U zU^0b(Nir+GpKyUmW&JpV$pTq#9JF(z&X4Sq8I6=Dm?$I>G?q1@%*7pXdPsLQwB$o35ys1U^pR+wns7`L zIfexXM2>N;KruN}kGiVd+S=w6P;7NA`z@Fl4(bi}JNHT-Vf5>ydr5lf1_$)F<^BX% zM3d!#`7bN3Dv$#*sbM@%l9p0)fQ9LcT!l$GlDovHo_5o$kdXluH%uWkDfxmk?u1Rp zz$0O&WN-v}u7Ut`xB2!%F*j1>E*Y9qLoq~26xHyFz%WS+^&QH9^eZd)*Wt(h)5>_I zkitgMx`u_y+L)qBwqDLEl7ZOXWGf%kCEin#_*yosRo5hDEV2Q5LB(r07B?cyN35~< z?3WtNPtfPTUA8FT^@;Aja|mk%|Nr*I%*ydspt;b#uw7$C{@UykyuL=EK5X92)Jb6# zTB;E8cY!^v(nQ-2w4e@WSYaY*X<)YUeG@tzKqQSwASHK!E{-Oa9NN1X=Gy!amYDL< znDDx&#}?IL@Z|*{_Red$`^oTdxHJVm_Ix^w_3m_gyyAvCaCyETU(CZb=i_K{dDoMv zx7+_`etUm+7{}rq9U&?tBe?sZ{Lb0pXx&_1d;&%M z8YIw1Q-OWs%0QzMLP2cNWq)T9A2L;<*pJz74#d^Q`mcQtRt%Tt4RmW2NPwV{Ut?`jxsVZ${7xs!_|K#zjbAy{I zkei(4isUYnK(>=0w}(JWJT4~CTOZquXcvbSHzaE7=0-jjZ8M_iB{JyZRnP!ZWk6C4 znWM{E!L&lfury{6$d>JPrRQA1mEwx;+~oxbNZ0mtmIYO}U| z_fx?=yJ(T&dbL8@VZNX!yMTrJ7&o0hlPX-5~_}`DxP!twlNX8bhYsu_J z(R?^jFjC}Tpmnvwd+AChRcZ%K-q)qX)%rK6PPMx^%;g}Kb$DEQmG0dNmoT&TZq6am z4UJC+_aE8v==6vkhQ0s(as{y%&PElRECKwWFT&%>MNZt2ASn#Rgy!!RJfAHm$l_qs zr8yZgKK|SpV5{6i44OQj6l@7Fn8=>J;VXx;t|c3Fp3ZZ_ zToQjOvap{AB`0OVMt3Vw_2D8s5|$6!RYO^fAiBC&t0X_1F}S~uev;Fr;YR&x>bdZe7mNa&qvUfauqwa^&bDFbp58yU~Kp+4=Z(cFuyEe3`hz zN;BBQl+~Xh4a5f!^@!{GgQu)CFnu265 z#DWS3euvq~bGt*Dlm?WSvNE$vDX-VnVs;K${Rt=i8$27I+?1P4_K4@w4acUK8Hf|I zz$J-ugrI@9(d_lg;beA~J#ULoaMDq#%I9JWUG3aTY{@B8srV?{Wdm2PLa=)-_g3VR z!Be95*WJ=|rb>e=9k2HxI3w!L`C$0%jGc%lY1jDSeyTry&`JZecHtKtv?`z5d;~{C zyguf1zi<;R?h1kh?c_cVvP^jH$O<5=oWsVpk5utY+peFSOBAIr$ZDF29vG{$5b#ii z)n~RET1U(=rrvRJFk&7geGltF^4+j-0f~x_GI(QtJ@dh90;7T+jI{&^2T`U3KlI@e zvw&HkLXR-*woKkwwDxpO*xA7sBR_!%WupFw9H7s*o8uErQ4CQO&uZxZSJ+vGMYXjJ zctE;4MNm>e5JV6}1VKu=8-|XdLy(j~8bnGOq`SLIx|;z>>8_y!zVY~;!+GcPoOk`; zg1KhjJbUlOu=caobFYE#3?}=7upLg~@vrXHEatermdIkaPB@=Ei7gAr^>Lsuz&c_{ zkwe#gP!{kYAtDJDtAxN#S8Tq z@U8&0x_aAu^8*cygwKdq{6{<+dL40b-4Dp*&m_&T4)3E$;-i(JP>gy&3xgJAg|^=w z1zbM3*V*z)kJaSM|=x&M~&(syPvD>YyLONz4L-C!vNVGYD0 z^M@Wrz$9KHCnXHq|~Zfy^To(dCua?PXY>YEQ2QZ+VYF>#`6iZqt@>!rOu$N zX;2aMWaR6rH5!dz=rC$Wo*$kHT)_WlRT7rptAo*&Rp=jyX&QRh!lJjP~cI^1i4dk=S6 zFmqZ3h@n(UQl%#?q@I?v-WhgNi0zCbn#XRuu5UOFsNWI2Ge7JL8G%WcA3 z%YjF{vu};NUbadY`GyPwXBh1_0K05lAVMOJ^x-CO#!jSdt-Jp!767&NZBJ=`;@bloxHmh+$<3@cjjv4nCe9zd(3 zVjpH@$LKtd4M;R8L5XC^v}HWHW1f-FNfKQJX@GJNvgpp43vL{$IKRA%i6|J`&UhBq z10=YNpKZrtyLq`^nF|fH0`oH^O&=C z^qI0?CZI*xb_|M9Im%o!)gQZ%h>NUVhCTj-G%(f8sS2uwuGa2$N~H2u2dwG| zb4^xLn=}+Dt5;DfAHREoBh7py7QAV@M4 zQ3>_Z(2kZPH0|Y}vSyiSvW{fqtF|~`WIkt#Da2;;bXCi)KSb&wWRl(A0YF=^O0hk3 zBi=DTYbwX+9eGR>XC~O+zW#p5O`!XsH=j1o5?u-o#EUN*VGJMCKHdMOSK_eJ4y{RB z3U!R1DFb~J;UfEKdRly%%wRl{oSL{0-=(*MRd1Ng%=veFG27*<*B2|}2fxEldeL7eG)KMF zagDcdqj}<&;&_Pf(DqKh(QWSzNpCKrXK@QaQSF^3c@X<7uTsZA%MXYF_xVLrk- z|98Q(`N)sFd4n-5{G%==;81bY#2T=W2^Vf{no1hZuEW@)2SvOD>pMH?C4f1*ZS*Vc z)pKCcd-_x3?mBwI(a)) z8a2T;;yJ5r%;2mtYGyqip z$>Wd#QPrXvj9&ZibdO(7^!3oNbQgnz8PInqE=2H9K;w+wkA03H4&nqB&x=Ad&s5a+ z>1{I~F_Zd6H@Jbv^=eZ3${^`fA{0|rPBdby)gMI5_)T58zE6)H@CAOG?oNZx$8@p1 zbN!i|zx*GGNiGhapU;bl(Zg1-e2nkjDN(d5Can_z8k$0m!FaCYBZV*q%eY*;1UBXp zdHtf}9a7X;y7ri%c&7Hw>!zd2vzUz&wr^gZbiIV4CV1|ZG&EfLTRsAVDGRpq;K}PO zv;AoM@QY+X3|q5sOgMSHiF{X(`p`{C$+tU|J11U#eqY-vZ=_IzL>&arCaD5^5;#oE zVu+5^?o#_vePx}mM7ppqCBZiA{!s+V$x7?} zzOUI++$1Z=>(ex&ZJpJCs6zL-z&I4r3{$>#GSYf2ha$0hCxB8p+-HZ#AyLQ`>$)GO z;|zzyLKUbZTN4G7I0<<4X6+wC_W{d%3^71g(EShYZoKre3JQKAYh+?Qj|UCas1xg7K1R9f%f(#7&bQIMm?{NH9BL;6*kG-WfGF zFy?0`3wj|+QTUF8R>GPTw@%8`F!pdrsV!|m^Zr-4&r06c1u6151&8QkpG9R%-4aGQ z-Pq#L*lT6#fki1A=z0xxBY-OLZV=$G2fBDqt9j}vxn4ylk?#-WD}Ik21ECWU-Ix?t z)<@%Yg!ZN1imM{!uE7No4Z4%hxv^C#?Pw1LLA@BzSW)ikkacYx2=q)+LS^^sLEzWz zt1bLJu(&GcPS1iyvw*JrqbG7(WGs5>S`_HmyuEWQDHmg`7K*s`zAFb2(o$A5P9c^} zio#J*T3bk|1N!FY^gY*zhXZqh?%x8MWSMyJ_nyVQ=ug$x^JLfIXkm;q;2k`_j}lTu zz?UPFMerooT*F*?d?z=MU2$B?KZ<#i*<6=_k27=ALD`W_@gz96IxlxdqQX&0^Vu?V z%Dwsl<#c{v+=MnpMI!m<>Nsw##oEuLU-HZGQR7x?1=U>_q3y}16)6_3z6d98L9+deVC5=y%(Da$d$x*RM*am+zn*) z*;CE;Ly5mSdrs9c$u}h_q+tCpzx3*8>d#JmEOFIbc31Q%g>pADm+H~Oox7(Sk4wy= zjt6P>b<~{h`Xm~7%T-*4%YYvncP1`(9S}?&yxhj!=#reRrj%KZ^Uk`OPdxZuKs-8B z){2{G5Tg^qM9L*@bw=k*$mqU{0!4l3ZUUipycm09oTw$}fe5B@ar8ra-;5TD^;jbUV9P984Lj-m_YX;Iaa^3>G^H;XGHO-dR z;?oAys)mXwZaN4srX38*=9}SBCC~xIugHSA?K&i4db-|0>55Z%aOyGyrj|WPD(r2A z#O8ICvR7#^Sq5kMU7ppy$L5gbmSZ-h~1&@k=oYL+;X1b6Dy z@u;k;@vs?@ux+*Ub)wli&1JVyGD@;m<=@e~^RV}!BR0xHLG+eeYFi_rw6?{7%S$mq zqt2RH`_3i-`}#NLHT2V$*+E9BEEyg477TP9+EPm3NJ!yeKIP_wc<*6p%V2?R4!I-# zB_XypJ#mPeXy+KUcS{0gr{j`K&QXOfpZh7NgRkSVc2GR~EDZ`a8-q&wXFH^z*1F_= zQwM@-p_tRF4|Hdqy6X*gG%Aur+WkY#eaxKLbn!mo7UebpIq6(+HP2XqG-4VXvL-0n zr3L_f3PMT4s2GfnS`;tAh<$|Rm9<=;U=!9OHR0MXU1}2nxiK_ByjQIt+(q6g;at;ph>-L3;Gn7W>bd(@>qrvEtO4MzN@mUOx~)Ky>0@<%f3Vr`Q*Nb= z1PytDOnIvD6PE~Wug}lW9(47{w~apC*fhn3<|tAZPTPS3wusng0ZJxkMK<3W$kUad zr7Q2=G2`I0mkJ`rzd{_tD0ag%r$!uGojG!PuTjs}jdrM+2-RGv;@fzeFs~s;xqUCY zpUQz!(30}_Ioa`}Y^4$TF! zlvU~SO~Z5bBsj2kKbx)B=wWdyetw~=OwfkRPgFVata4%(T!6lM>JtEQmQ}MTIeACW z$X%&2fmV#_JcNL+95E?bR{WZ_;_zy{N}??FQ`0injQ*D1Y4M4}k|FAhSK&M)FpRC)2wMU}Xpb5Luyq%(b&A-`*Hpur))C+P z4%2HgV!r~RIPUr31cKhTb_R`H8s2So$3TCKq-M*wipk<5idPJsa%q)ri@&~%A1vha{g9hUzZBcq>S&}v zVL70JVl=MrQ{Z&>hiCr%?ym*Hpz7-C4m2h96?hQjB!MRYt;w79v)9Bwoy$!Gg&5t&)ae4kYC8#h#edCxzj%_q4Q$ z&B3ql8asTljlUCsH>MNpv}GRlyvVoY-P6d+f{rZT zalGx9@%g@9FBH`u<<^IWXHYaeeTw`#n_FJ7IVmT{GS@g`(^^^{fo)nu9>Fy;yRqa& z8iOl_M{`9$93oUoS~z_b_>q+B22TD^3(}{MLq6&cqz&`r;kw_&sk43Wft9)$?o}O#PSSJ_qZ?9?960x!04!)Tq>L&S6rc zWzI2oujKQn>bE@6f8;yyN_2R-zHI@1qaqUDte8G6BPq_G45ek4HZfFP{N_&7Q58DF zJffKtUmd+q7w|7E|`J16i z!)QH>R5fLl1n$W#VNLZ7xgLh4p?5`4z1xN`25tR{t^w_ZHO~*+9z!WGmm&u)te#1MLGB7tRIJ1!f%adYnW()w(}UcPQOsG)NYyaVVD68i zZf|kj>YbvnUqjD_aH;Cj)Z|_zUZ6L=)#w@DLzGv8vWiYIaCij;=p zD~bF7>5JBC;!vgAd$b2!ig#BM!(CHM>(HS%qMM`(*$!Q@?VsM^-?hcbV9oHc7(f!r zXU-P{(9?rs-e-T8(g)r7;Dq;>Fj8OoA+c+U`SWYMNF+zWk82(GeK@+Q3W*hcRuiMg zhck5sHBaN)FKc2qUB2ZI-4fHlrE#b+6m+bDvfaxZIZ^xV;>GXJ+UJH$SIX4Befdl zs6D{+^ZZWzB2Z;NQhXpi#)jp!scR`FWNJ$!KHG!!(N9%5=$1D z1=l>PrKWUoBe7vC$yz)92Zm`J=S$+w4R*N%#S2%WH6mV(7954g?Yyt#Rs`MisR}ZD zu2>1QAUhBNqj43&ZQTS_0hyaeHX>a?phKtZyzTR+)gNCC3x}x$n?FGFiEx)ZR7iT6 zl6yhS2-~@vz4}M-fywS_ZZfYBD_~x-;F&xbbeHH*25g5+ACk#U$RS-bmlX=JL#gW( zhJ5*IAmc^)O8;S102K|C4|klyP(Wg-2Be!NIK#vUVrC!%OiT7#$%aFJRt{zCXkkrE<)i*dw*>Wap6Di5gE`96 z76-Oq9>*IazN*y-TZ7S!*!LW!qSNM(O0wzj%Ds?T#nGg=twL9Q4U^Tr>aK*OSXu2D z()7AmjmEAsUQgrsc}8>SDsxjk;~r&UZy8JqCu(|bWRojwEKg6nE)-)MZ52n&G+o`wu4~Ar*OE5KPmdzp z73{+YaiMW>yJ&OZ7eaeo4pz+{NuOkMtwEW~cgy(u^a0N4(VfzyAX|)>_1Z7)zPz<6 zC_}uZgOfGZcGIEu)1te|p@}*1UXL_-8<{JEOs?j3a&&o4-;f^1FB%;sz%oiv)$bB_ zMs!%E%k=a*mK{94>^$>N&7cIW%hvnrvE)^~r7mRipi99cNV1|?!KLg=yF!VCqzb9& z;<{(}9Fw|EuPp5M&01ROZM!Dzquhaz$>EYI=1{on4x>b486VmR`W-Pf>bVYS)ly#S z%Dq8=rP+vJ|7+0z=%2#n@@tg+QDCoRV*mhzuvdRJn8WHnO;gw!=CieO&{cAQxe>L0 zW*y)Gj7!!4eb^ED$N&H=dG|;A1>k{Uf2j)p&^5syc)B%OssnpxE$nTS|8-z1Y+;tA zkpmE>VFa=pSlb!N*}>F|b`EAn_P>ij^&@O=M;Jg12BZ2fkUA`;zd)96|2N8-o%6UJ zhCqU$82%dt+qU{2QLmk07s(!`jQm}Ao)dACwJ;VNjQP_U`m@2lg*A29$mbuK3PvC! zpuN$bBOv&zSH<*<)&~awXu<^mNdBPx`9&R;Li&s9VCrb8Zv`|1vDsLez%xF5kCCr} zUHt}F9OVBw{8wc7unoHZa%3tRm>L;6f{cV=cj9*^c)`1BI0nOj(cy384UGE#Fs}{G z9DYe#;o}*VyjR!@3t$s=20xXwKijW(uweJ~FVL?(1;1tZZ~ItM&0bJBj6wy&(Eq{v zlSmF*0uD#~f70pSA`{Q>f=yv(dIaOauF@avSCDu#e?#gU8X6e`9YGGi?XvJ2q5q`( zf)qUbI|O8HU||ST-r4~ze|P2Kcapuuls*0%=C2d`qW}f|s>4gTZy^nDeh2xd9trQp zzU5d+{f+Z`Q#O21@B-&s7>Uf^V878h!;|1W$G0S2n4R{oF7T(6_BYmJcp|){_?9^E zJK{eni{Y{GZsA+3mE3Ov{Rh)9JQdyvd`rcW{|yz+5DZU)clzGa;(kZ_Z^JJ<3|`86 z3u93DO=$nF=!J*FYjkhn!+(e0>KT78)rH5xduea6lQ5a~U%lxT`*%w%JPckMc?Audc-P=9_gLdM+&^n*erX!vVeqxxx3FQ$e}Vm~ z^oEDQm#W^vWNrTi_N#Ce9tK~;cMJ1&_!ro(l0JACe5KJXjM@3$V1Lvd!NcH7Hf~{T zFbnx@CxKHx{wm;rhrusi(41NOm7S{Rk zpJD%*4u*%p&!pYLdc*%2_UF7BJPkfcbxUiE{~HZ1WhM6p88&46IgDfgn860d0%<>g F`agbzv+e)@ diff --git a/src/LQR_Goal_1.m b/src/LQR_Goal_1.m deleted file mode 100644 index 34baaa8..0000000 --- a/src/LQR_Goal_1.m +++ /dev/null @@ -1,284 +0,0 @@ -% Clear workspace -clear all; close all; clc; - -% Parameters source: https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf -g = 9.81; m = 0.468; Ix = 4.856*10^-3; -Iy = 4.856*10^-3; Iz = 8.801*10^-3; - -% States: -% X1: x X4: x' -% X2: y X5: y' -% X3: z X6: z' -% X7: Pitch angle (x-axis) X10: Pitch rate (x-axis) -% X8: Roll angle (y-axis) X11: Roll rate (y-axis) -% X9: Yaw angle (z-axis) X12: Yaw rate (z-axis) - -% Inputs: Outputs: -% U1: Total Upward Force (along z-axis) Y1: Position along x axis -% U2: Pitch Torque (about x-axis) Y2: Position along y axis -% U3: Roll Torque (about y-axis) Y3: Position along z axis -% U4: Yaw Torque (about z-axis) Y4: Pitch (about x-axis) -% Y5: Roll (about y-axis) -% Y6: Yaw (about z-axis) - -% State Space Source: https://arxiv.org/ftp/arxiv/papers/1908/1908.07401.pdf -% X' = Ax + Bu -% Y = Cx - -nStates = 12; -nInputs = 4; -nOutputs = 6; - -A = [0 0 0 1 0 0 0 0 0 0 0 0; - 0 0 0 0 1 0 0 0 0 0 0 0; - 0 0 0 0 0 1 0 0 0 0 0 0; - 0 0 0 0 0 0 0 -g 0 0 0 0; - 0 0 0 0 0 0 g 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 1 0 0; - 0 0 0 0 0 0 0 0 0 0 1 0; - 0 0 0 0 0 0 0 0 0 0 0 1; - 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0]; - -% Note: In paper, 1/m is in wrong spot -B = [0 0 0 0; - 0 0 0 0; - 0 0 0 0; - 0 0 0 0; - 0 0 0 0; - 1/m 0 0 0; - 0 0 0 0; - 0 0 0 0; - 0 0 0 0; - 0 1/Ix 0 0; - 0 0 1/Iy 0; - 0 0 0 1/Iz]; - -C = [1 0 0 0 0 0 0 0 0 0 0 0; - 0 1 0 0 0 0 0 0 0 0 0 0; - 0 0 1 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 1 0 0 0 0 0; - 0 0 0 0 0 0 0 1 0 0 0 0; - 0 0 0 0 0 0 0 0 1 0 0 0]; - -D = zeros(6,4); - -continuous = ss(A, B, C, D); -T_s = 0.01; -discrete = c2d(continuous, T_s); - -%Check if this works -impulse(discrete, 0:T_s:1); - -%We should see that U1 gets us only translation in z, U2 couples Y2 and Y4, -%U3 couples Y1 and Y5, and U4 gets us Y6 - -%% Define goals -%Goal 1: settle at 1m height <2s -x_0_up = [0, 0, -1, ... - 0, 0, 0, ... - 0, 0, 0, ... - 0, 0, 0]'; %Redefine origin! - -%Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot -x_0_pitchroll = [0, 0, 0, ... - 0, 0, 0, ... - 10*(pi/180), 10*(pi/180), 0, ... - 0, 0, 0]'; %Pitch and roll of 10 degrees - -x_0_roll = [0, 0, 0, ... - 0, 0, 0, ... - 0, 10*(pi/180), 0, ... - 0, 0, 0]'; %Roll of 10 degrees - -%Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds. -x_0_trans = [-1, -1, 0, ... - 0, 0, 0, ... - 0, 0, 0, ... - 0, 0, 0]'; %Redefine origin! - -%Define Q and R for the cost function. Begin with nominal ones for all. -Q = diag([1000, 1000, 1000, ... % x, y, z - 1, 1, 100, ... % x', y', z' - 200, 200, 1, ... % roll, pitch, yaw - 1, 1, 1]); % roll', pitch', yaw' - -R = diag([10, 20, 20, 1]); % upward force, pitch torque, roll torque, yaw torque -%% Finite-Time Horizon LQR for Goal 1 - -%Calculate number of timesteps. -tSpan = 0:T_s:2; -nSteps = length(tSpan); - -%Determine gains -[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); -FiniteLQR_Goal_1_K = K; -save('FiniteLQRGoal_1_K.mat', 'K'); - -%Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B); -%States are relative to origin, so we need to add the reference to the -%state to get global coordinates -xlqr(3,:) = xlqr(3,:) + 1; -%Plot -plot_states(xlqr, tSpan); -zd = diff(xlqr(6,:))./T_s; - -%Simulate nonlinear model -[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); -set_param('LQRNonlinearSim', 'StopTime', '2') -simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); - -time = simout.allVals.Time(:,1); -figure(); -plot(time, simout.z+1, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; -plot(time, xlqr(3,:), '-b','LineWidth', 1); -xlabel('Time (s)'); -ylabel('Z (m)'); -legend({'Nonlinear Model', 'Linear Model'}); - - - -%% Finite-Time Horizon LQR for Goal 2 -Q = diag([0, 0, 0, ... % x, y, z - 0, 0, 0, ... % x', y', z' - 200, 200, 1, ... % roll, pitch, yaw - 10, 10, 1]); % roll', pitch', yaw' - -%Calculate number of timesteps. -tSpan = 0:T_s:4; -nSteps = length(tSpan); - -%Determine gains -[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); - - -%Pitch Goal -%Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitchroll, K, discrete.A, discrete.B); - -%Plot -plot_states(xlqr, tSpan); -yd = diff(xlqr(5,:))./T_s -pd = diff(xlqr(7,:))./T_s - -%Propagate -%[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B); - -%Plot -%plot_states(xlqr, tSpan); -%xd = diff(xlqr(4,:))./T_s -%rd = diff(xlqr(8,:))./T_s - -%Simulate nonlinear model -[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); -set_param('LQRNonlinearSim', 'StopTime', '4') -simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); - -time = simout.allVals.Time(:,1); -figure(); -plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; -plot(time, xlqr(8,:), '-b','LineWidth', 1); -%plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; -xlabel('Time (s)'); -ylabel('Pitch/Roll Angle (rad)'); -legend({'Nonlinear Model', 'Linear Model'}); - -%% Finite-Time Horizon For Goal 3 -Q = diag([1000, 1000, 0, ... % x, y, z - 0, 0, 0, ... % x', y', z' - 200, 200, 1, ... % roll, pitch, yaw - 0, 0, 1]); % roll', pitch', yaw' -%Calculate number of timesteps. -tSpan = 0:T_s:5; -nSteps = length(tSpan); - -%Determine gains -[K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps); - -%Pitch Goal -%Propagate -[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B); -xlqr(1:2,:) = xlqr(1:2,:) + 1; - -%Plot -plot_states(xlqr, tSpan); - -%Simulate nonlinear model -[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); -set_param('LQRNonlinearSim', 'StopTime', '5') -simout = sim('LQRNonlinearSim', 'FixedStep', '.01'); - -time = simout.allVals.Time(:,1); -figure(); -plot(time, simout.x+1, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; -plot(time, xlqr(1,:), '-b','LineWidth', 1); -%plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on; -xlabel('Time (s)'); -ylabel('x (m)'); -legend({'Nonlinear Model', 'Linear Model'}); - -%% Helper Functions - -function [K, P] = LQR_LTI(A, B, Q, R, nSteps) - %Set P up - P = zeros(size(Q, 1), size(Q, 2), nSteps); - %Initial value of P - P(:, :, nSteps) = 1/2 * Q; - %Set K up, initial K is 0, so this is fine. - K = zeros(length(R), length(Q), nSteps); - - for i = nSteps-1:-1:1 - P_ = P(:,:, i+1); - - K(:, :, i) = ( 1/2 * R + B' * P_ * B )^(-1) * B' * P_ * A; - P(:, :, i) = A' * P_ * ( A - B * K(:, :, i) ) + Q * 1/2; - end -end - -function [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0, K, A, B) - %Set up for propagation - ulqr = zeros(nInputs, nSteps); - xlqr = zeros(nStates, nSteps); - xlqr(:, 1) = x_0; - - for i = 1:(nSteps - 1) - ulqr(:,i) = K(:,:,i) * xlqr(:,i); - xlqr(:,i+1) = (A*xlqr(:, i) - B*ulqr(:, i)); - end -end - -function plot_states(xlqr, tSpan) - figure(); - subplot(1, 2, 1); - plot(tSpan, xlqr(1, :), '-r', 'LineWidth', 2); - hold on; - plot(tSpan, xlqr(2, :), '-g'); - plot(tSpan, xlqr(3, :), '-b'); - plot(tSpan, xlqr(4, :), '--r', 'LineWidth', 2); - plot(tSpan, xlqr(5, :), '--g'); - plot(tSpan, xlqr(6, :), '--b'); - legend('x', 'y', 'z', 'x`', 'y`', 'z`'); - title("Translations(-) and Velocities (--)"); - xlabel("Time(s)"); - ylabel("Displacement (m)"); - - subplot(1, 2, 2); - plot(tSpan, xlqr(7, :), '-r'); - hold on; - plot(tSpan, xlqr(8, :), '-g'); - plot(tSpan, xlqr(9, :), '-b'); - plot(tSpan, xlqr(10, :), '--r'); - plot(tSpan, xlqr(11, :), '--g'); - plot(tSpan, xlqr(12, :), '--b'); - legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate'); - title("Angular Displacements(-) and Velocities(--)"); - xlabel("Time(s)"); - ylabel("Displacement (rad)"); -end - -function [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(X0) -x0=X0(1); y0=X0(2); z0=X0(3); xdot0=X0(4); ydot0=X0(5); zdot0=X0(6); phi0=X0(7); theta0=X0(8); psi0=X0(9); phidot0=X0(10); thetadot0=X0(11); psidot0=X0(12); -end