diff --git a/dcm-simscape.html b/dcm-simscape.html index a999d8f..0fab696 100644 --- a/dcm-simscape.html +++ b/dcm-simscape.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + ESRF Double Crystal Monochromator - Dynamical Multi-Body Model @@ -39,44 +39,51 @@

Table of Contents

@@ -94,44 +101,53 @@ In this document, a Simscape (.e.g. multi-body) model of the ESRF Double Crystal It is structured as follow:

-
-

1. System Kinematics

+
+

1. System Kinematics

- +

-
-

1.1. Bragg Angle

+
+

1.1. Bragg Angle

-
-
%% Tested bragg angles
-bragg = linspace(5, 80, 1000); % Bragg angle [deg]
-d_off = 10.5e-3; % Wanted offset between x-rays [m]
-
-
+

+There is a simple relation eq:bragg_angle_formula between: +

+
    +
  • \(d_{\text{off}}\) is the wanted offset between the incident x-ray and the output x-ray
  • +
  • \(\theta_b\) is the bragg angle
  • +
  • \(d_z\) is the corresponding distance between the first and second crystals
  • +
-
-
%% Vertical Jack motion as a function of Bragg angle
-dz = d_off./(2*cos(bragg*pi/180));
-
-
+\begin{equation} \label{eq:bragg_angle_formula} +d_z = \frac{d_{\text{off}}}{2 \cos \theta_b} +\end{equation} + +

+This relation is shown in Figure 1. +

-
+

jack_motion_bragg_angle.png

Figure 1: Jack motion as a function of Bragg angle

+

+The required jack stroke is approximately 25mm. +

+
%% Required Jack stroke
 ans = 1e3*(dz(end) - dz(1))
@@ -144,34 +160,34 @@ dz = d_off./(2*
 
-
-

1.2. Kinematics (111 Crystal)

+
+

1.2. Kinematics (111 Crystal)

The reference frame is taken at the center of the 111 second crystal.

-
-

1.2.1. Interferometers - 111 Crystal

+
+

1.2.1. Interferometers - 111 Crystal

Three interferometers are pointed to the bottom surface of the 111 crystal.

-The position of the measurement points are shown in Figure 2 as well as the origin where the motion of the crystal is computed. +The position of the measurement points are shown in Figure 2 as well as the origin where the motion of the crystal is computed.

-
+

sensor_111_crystal_points.png

Figure 2: Bottom view of the second crystal 111. Position of the measurement points.

-The inverse kinematics consisting of deriving the interferometer measurements from the motion of the crystal (see Figure 3): +The inverse kinematics consisting of deriving the interferometer measurements from the motion of the crystal (see Figure 3):

\begin{equation} \begin{bmatrix} @@ -185,7 +201,7 @@ d_z \\ r_y \\ r_x \end{equation} -
+

schematic_sensor_jacobian_inverse_kinematics.png

Figure 3: Inverse Kinematics - Interferometers

@@ -193,7 +209,7 @@ d_z \\ r_y \\ r_x

-From the Figure 2, the inverse kinematics can be solved as follow (for small motion): +From the Figure 2, the inverse kinematics can be solved as follow (for small motion):

\begin{equation} \bm{J}_{s,111} @@ -213,7 +229,7 @@ J_s_111 = [1, 0.07, -0.015
- +
@@ -245,7 +261,7 @@ J_s_111 = [1, 0.07, -0.015
Table 1: Sensor Jacobian \(\bm{J}_{s,111}\)

-The forward kinematics is solved by inverting the Jacobian matrix (see Figure 4). +The forward kinematics is solved by inverting the Jacobian matrix (see Figure 4).

\begin{equation} \begin{bmatrix} @@ -259,13 +275,13 @@ x_1 \\ x_2 \\ x_3 \end{equation} -
+

schematic_sensor_jacobian_forward_kinematics.png

Figure 4: Forward Kinematics - Interferometers

- +
@@ -298,15 +314,15 @@ x_1 \\ x_2 \\ x_3 -
-

1.2.2. Piezo - 111 Crystal

+
+

1.2.2. Piezo - 111 Crystal

-The location of the actuators with respect with the center of the 111 second crystal are shown in Figure 5. +The location of the actuators with respect with the center of the 111 second crystal are shown in Figure 5.

-
+

actuator_jacobian_111_points.png

Figure 5: Location of actuators with respect to the center of the 111 second crystal (bottom view)

@@ -327,14 +343,14 @@ d_z \\ r_y \\ r_x \end{equation} -
+

schematic_actuator_jacobian_inverse_kinematics.png

Figure 6: Inverse Kinematics - Actuators

-Based on the geometry in Figure 5, we obtain: +Based on the geometry in Figure 5, we obtain:

\begin{equation} \bm{J}_{a,111} @@ -354,7 +370,7 @@ J_a_111 = [1, 0.14, -0.1525
-
Table 2: Inverse of the sensor Jacobian \(\bm{J}_{s,111}^{-1}\)
+
@@ -400,13 +416,13 @@ d_{u_r} \\ d_{u_h} \\ d_{d} \end{equation} -
+

schematic_actuator_jacobian_forward_kinematics.png

Figure 7: Forward Kinematics - Actuators for 111 crystal

-
Table 3: Actuator Jacobian \(\bm{J}_{a,111}\)
+
@@ -440,8 +456,8 @@ d_{u_r} \\ d_{u_h} \\ d_{d} -
-

1.3. Save Kinematics

+
+

1.3. Save Kinematics

save('mat/dcm_kinematics.mat', 'J_a_111', 'J_s_111')
@@ -451,15 +467,15 @@ d_{u_r} \\ d_{u_h} \\ d_{d}
 
-
-

2. Open Loop System Identification

+
+

2. Open Loop System Identification

- +

-
-

2.1. Identification

+
+

2.1. Identification

Let’s considered the system \(\bm{G}(s)\) with: @@ -470,11 +486,11 @@ Let’s considered the system \(\bm{G}(s)\) with:

-It is schematically shown in Figure 8. +It is schematically shown in Figure 8.

-
+

schematic_system_inputs_outputs.png

Figure 8: Dynamical system with inputs and outputs

@@ -516,8 +532,8 @@ State-space model with 3 outputs, 3 inputs, and 24 states.
-
-

2.2. Plant in the frame of the fastjacks

+
+

2.2. Plant in the frame of the fastjacks

load('dcm_kinematics.mat');
@@ -525,11 +541,11 @@ State-space model with 3 outputs, 3 inputs, and 24 states.
 

-Using the forward and inverse kinematics, we can computed the dynamics from piezo forces to axial motion of the 3 fastjacks (see Figure 9). +Using the forward and inverse kinematics, we can computed the dynamics from piezo forces to axial motion of the 3 fastjacks (see Figure 9).

-
+

schematic_jacobian_frame_fastjack.png

Figure 9: Use of Jacobian matrices to obtain the system in the frame of the fastjacks

@@ -550,7 +566,7 @@ The DC gain of the new system shows that the system is well decoupled at low fre
-
Table 4: Inverse of the actuator Jacobian \(\bm{J}_{a,111}^{-1}\)
+
@@ -582,17 +598,17 @@ The DC gain of the new system shows that the system is well decoupled at low fre
Table 5: DC gain of the plant in the frame of the fast jacks \(\bm{G}_{\text{fj}}\)

-The bode plot of \(\bm{G}_{\text{fj}}(s)\) is shown in Figure 10. +The bode plot of \(\bm{G}_{\text{fj}}(s)\) is shown in Figure 10.

-
+

bode_plot_plant_fj.png

Figure 10: Bode plot of the diagonal and off-diagonal elements of the plant in the frame of the fast jacks

-
+

Computing the system in the frame of the fastjack gives good decoupling at low frequency (until the first resonance of the system).

@@ -601,11 +617,11 @@ Computing the system in the frame of the fastjack gives good decoupling at low f
-
-

2.3. Plant in the frame of the crystal

+
+

2.3. Plant in the frame of the crystal

-
+

schematic_jacobian_frame_crystal.png

Figure 11: Use of Jacobian matrices to obtain the system in the frame of the crystal

@@ -660,19 +676,102 @@ The main reason is that, as we map forces to the center of the 111 crystal and n
-
-

3. Active Damping Plant (Strain gauges)

+
+

3. Open-Loop Noise Budgeting

- + +

+ +
+

noise_budget_dcm_schematic_fast_jack_frame.png +

+

Figure 12: Schematic representation of the control loop in the frame of one fast jack

+
+
+ +
+

3.1. Power Spectral Density of signals

+
+

+Interferometer noise: +

+
+
Wn = 6e-11*(1 + s/2/pi/200)/(1 + s/2/pi/60); % m/sqrt(Hz)
+
+
+ +
+Measurement noise: 0.79 [nm,rms]
+
+ + +

+DAC noise (amplified by the PI voltage amplifier, and converted to newtons): +

+
+
Wdac = tf(3e-8); % V/sqrt(Hz)
+Wu = Wdac*22.5*10; % N/sqrt(Hz)
+
+
+ +
+DAC noise: 0.95 [uV,rms]
+
+ + +

+Disturbances: +

+
+
Wd = 5e-7/(1 + s/2/pi); % m/sqrt(Hz)
+
+
+ +
+Disturbance motion: 0.61 [um,rms]
+
+ + +
+
%% Save ASD of noise and disturbances
+save('mat/asd_noises_disturbances.mat', 'Wn', 'Wu', 'Wd');
+
+
+
+
+ +
+

3.2. Open Loop disturbance and measurement noise

+
+

+The comparison of the amplitude spectral density of the measurement noise and of the jack parasitic motion is performed in Figure 13. +It confirms that the sensor noise is low enough to measure the motion errors of the crystal. +

+ + +
+

open_loop_noise_budget_fast_jack.png +

+

Figure 13: Open Loop noise budgeting

+
+
+
+
+ +
+

4. Active Damping Plant (Strain gauges)

+
+

+

In this section, we wish to see whether if strain gauges fixed to the piezoelectric actuator can be used for active damping.

-
-

3.1. Identification

-
+
+

4.1. Identification

+
%% Input/Output definition
 clear io; io_i = 1;
@@ -730,15 +829,15 @@ G_sg = linearize(mdl, io);
 
 
 
-
+

strain_gauge_plant_bode_plot.png

-

Figure 12: Bode Plot of the transfer functions from piezoelectric forces to strain gauges measuremed displacements

+

Figure 14: Bode Plot of the transfer functions from piezoelectric forces to strain gauges measuremed displacements

-
+

-As the distance between the poles and zeros in Figure 15 is very small, little damping can be actively added using the strain gauges. +As the distance between the poles and zeros in Figure 17 is very small, little damping can be actively added using the strain gauges. This will be confirmed using a Root Locus plot.

@@ -746,23 +845,23 @@ This will be confirmed using a Root Locus plot.
-
-

3.2. Relative Active Damping

-
+
+

4.2. Relative Active Damping

+
Krad_g1 = eye(3)*s/(s^2/(2*pi*500)^2 + 2*s/(2*pi*500) + 1);
 

-As can be seen in Figure 13, very little damping can be added using relative damping strategy using strain gauges. +As can be seen in Figure 15, very little damping can be added using relative damping strategy using strain gauges.

-
+

relative_damping_root_locus.png

-

Figure 13: Root Locus for the relative damping control

+

Figure 15: Root Locus for the relative damping control

@@ -772,9 +871,9 @@ As can be seen in Figure 13, very little damping can b
-
-

3.3. Damped Plant

-
+
+

4.3. Damped Plant

+

The controller is implemented on Simscape, and the damped plant is identified.

@@ -818,20 +917,20 @@ G_dp.OutputName = {'d_ur', +

comp_damp_undamped_plant_rad_bode_plot.png

-

Figure 14: Bode plot of both the open-loop plant and the damped plant using relative active damping

+

Figure 16: Bode plot of both the open-loop plant and the damped plant using relative active damping

-
-

4. Active Damping Plant (Force Sensors)

-
+
+

5. Active Damping Plant (Force Sensors)

+

- +

Force sensors are added above the piezoelectric actuators. @@ -839,9 +938,9 @@ They can consists of a simple piezoelectric ceramic stack. See for instance fleming10_integ_strain_force_feedb_high.

-
-

4.1. Identification

-
+
+

5.1. Identification

+
%% Input/Output definition
 clear io; io_i = 1;
@@ -863,22 +962,22 @@ G_fs = linearize(mdl, io);
 

-The Bode plot of the identified dynamics is shown in Figure 15. +The Bode plot of the identified dynamics is shown in Figure 17. At high frequency, the diagonal terms are constants while the off-diagonal terms have some roll-off.

-
+

iff_plant_bode_plot.png

-

Figure 15: Bode plot of IFF Plant

+

Figure 17: Bode plot of IFF Plant

-
-

4.2. Controller - Root Locus

-
+
+

5.2. Controller - Root Locus

+

We want to have integral action around the resonances of the system, but we do not want to integrate at low frequency. Therefore, we can use a low pass filter. @@ -891,10 +990,10 @@ Kiff_g1 = eye(3)*1/ -

+

iff_root_locus.png

-

Figure 16: Root Locus plot for the IFF Control strategy

+

Figure 18: Root Locus plot for the IFF Control strategy

@@ -911,38 +1010,38 @@ save('mat/Kiff.mat', 'K
-
-

4.3. Damped Plant

-
+
+

5.3. Damped Plant

+

-Both the Open Loop dynamics (see Figure 9) and the dynamics with IFF (see Figure 17) are identified. +Both the Open Loop dynamics (see Figure 9) and the dynamics with IFF (see Figure 19) are identified.

We are here interested in the dynamics from \(\bm{u}^\prime = [u_{u_r}^\prime,\ u_{u_h}^\prime,\ u_d^\prime]\) (input of the damped plant) to \(\bm{d}_{\text{fj}} = [d_{u_r},\ d_{u_h},\ d_d]\) (motion of the crystal expressed in the frame of the fast-jacks). -This is schematically represented in Figure 17. +This is schematically represented in Figure 19.

-
+

schematic_jacobian_frame_fastjack_iff.png

-

Figure 17: Use of Jacobian matrices to obtain the system in the frame of the fastjacks

+

Figure 19: Use of Jacobian matrices to obtain the system in the frame of the fastjacks

-The dynamics from \(\bm{u}\) to \(\bm{d}_{\text{fj}}\) (open-loop dynamics) and from \(\bm{u}^\prime\) to \(\bm{d}_{\text{fs}}\) are compared in Figure 18. +The dynamics from \(\bm{u}\) to \(\bm{d}_{\text{fj}}\) (open-loop dynamics) and from \(\bm{u}^\prime\) to \(\bm{d}_{\text{fs}}\) are compared in Figure 20. It is clear that the Integral Force Feedback control strategy is very effective in damping the resonances of the plant.

-
+

comp_damped_undamped_plant_iff_bode_plot.png

-

Figure 18: Bode plot of both the open-loop plant and the damped plant using IFF

+

Figure 20: Bode plot of both the open-loop plant and the damped plant using IFF

-
+

The Integral Force Feedback control strategy is very effective in damping the modes present in the plant.

@@ -952,42 +1051,42 @@ The Integral Force Feedback control strategy is very effective in damping the mo
-
-

5. HAC-LAC (IFF) architecture

-
+
+

6. HAC-LAC (IFF) architecture

+

- +

-The HAC-LAC architecture is shown in Figure 19. +The HAC-LAC architecture is shown in Figure 21.

-
+

schematic_jacobian_frame_fastjack_hac_iff.png

-

Figure 19: HAC-LAC architecture

+

Figure 21: HAC-LAC architecture

-
-

5.1. System Identification

-
+
+

6.1. System Identification

+

Let’s identify the damped plant.

-
+

bode_plot_hac_iff_plant.png

-

Figure 20: Bode Plot of the plant for the High Authority Controller (transfer function from \(\bm{u}^\prime\) to \(\bm{\epsilon}_d\))

+

Figure 22: Bode Plot of the plant for the High Authority Controller (transfer function from \(\bm{u}^\prime\) to \(\bm{\epsilon}_d\))

-
-

5.2. High Authority Controller

-
+
+

6.2. High Authority Controller

+

Let’s design a controller with a bandwidth of 100Hz. As the plant is well decoupled and well approximated by a constant at low frequency, the high authority controller can easily be designed with SISO loop shaping. @@ -1018,28 +1117,28 @@ L_hac_lac = G_dp * Khac;

-
+

hac_iff_loop_gain_bode_plot.png

-

Figure 21: Bode Plot of the Loop gain for the High Authority Controller

+

Figure 23: Bode Plot of the Loop gain for the High Authority Controller

-As shown in the Root Locus plot in Figure 22, the closed loop system should be stable. +As shown in the Root Locus plot in Figure 24, the closed loop system should be stable.

-
+

loci_hac_iff_fast_jack.png

-

Figure 22: Root Locus for the High Authority Controller

+

Figure 24: Root Locus for the High Authority Controller

-
-

5.3. Performances

-
+
+

6.3. Performances

+

In order to estimate the performances of the HAC-IFF control strategy, the transfer function from motion errors of the stepper motors to the motion error of the crystal is identified both in open loop and with the HAC-IFF strategy.

@@ -1059,22 +1158,72 @@ It is first verified that the closed-loop system is stable:

-And both transmissibilities are compared in Figure 23. +And both transmissibilities are compared in Figure 25.

-
+

stepper_transmissibility_comp_ol_hac_iff.png

-

Figure 23: Comparison of the transmissibility of errors from vibrations of the stepper motor between the open-loop case and the hac-iff case.

+

Figure 25: Comparison of the transmissibility of errors from vibrations of the stepper motor between the open-loop case and the hac-iff case.

-
+

The HAC-IFF control strategy can effectively reduce the transmissibility of the motion errors of the stepper motors. This reduction is effective inside the bandwidth of the controller.

+
+
+
+ +
+

6.4. Close Loop noise budget

+
+

+Let’s compute the amplitude spectral density of the jack motion errors due to the sensor noise, the actuator noise and disturbances. +

+ +
+
%% Computation of ASD of contribution of inputs to the closed-loop motion
+% Error due to disturbances
+asd_d = abs(squeeze(freqresp(Wd*(1/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
+% Error due to actuator noise
+asd_u = abs(squeeze(freqresp(Wu*(G_dp(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
+% Error due to sensor noise
+asd_n = abs(squeeze(freqresp(Wn*(G_dp(1,1)*Khac(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz')));
+
+
+ +

+The closed-loop ASD is then: +

+
+
%% ASD of the closed-loop motion
+asd_cl = sqrt(asd_d.^2 + asd_u.^2 + asd_n.^2);
+
+
+ +

+The obtained ASD are shown in Figure 26. +

+ + +
+

close_loop_asd_noise_budget_hac_iff.png +

+

Figure 26: Closed Loop noise budget

+
+ +

+Let’s compare the open-loop and close-loop cases (Figure 27). +

+ +
+

cps_comp_ol_cl_hac_iff.png +

+

Figure 27: Cumulative Power Spectrum of the open-loop and closed-loop motion error along one fast-jack

@@ -1082,7 +1231,7 @@ This reduction is effective inside the bandwidth of the controller.

Author: Dehaeze Thomas

-

Created: 2021-11-30 mar. 15:17

+

Created: 2021-11-30 mar. 17:54

diff --git a/dcm-simscape.org b/dcm-simscape.org index c5ec5c5..c92dc12 100644 --- a/dcm-simscape.org +++ b/dcm-simscape.org @@ -56,6 +56,7 @@ In this document, a Simscape (.e.g. multi-body) model of the ESRF Double Crystal It is structured as follow: - Section [[sec:dcm_kinematics]]: the kinematics of the DCM is presented, and Jacobian matrices which are used to solve the inverse and forward kinematics are computed. - Section [[sec:open_loop_identification]]: the system dynamics is identified in the absence of control. +- Section [[sec:dcm_noise_budget]]: an open-loop noise budget is performed. - Section [[sec:active_damping_strain_gauges]]: it is studied whether if the strain gauges fixed to the piezoelectric actuators can be used to actively damp the plant. - Section [[sec:active_damping_iff]]: piezoelectric force sensors are added in series with the piezoelectric actuators and are used to actively damp the plant using the Integral Force Feedback (IFF) control strategy. - Section [[sec:hac_iff]]: the High Authority Control - Low Authority Control (HAC-LAC) strategy is tested on the Simscape model. @@ -95,17 +96,28 @@ It is structured as follow: #+end_src ** Bragg Angle -#+begin_src matlab +There is a simple relation eqref:eq:bragg_angle_formula between: +- $d_{\text{off}}$ is the wanted offset between the incident x-ray and the output x-ray +- $\theta_b$ is the bragg angle +- $d_z$ is the corresponding distance between the first and second crystals + +\begin{equation} \label{eq:bragg_angle_formula} +d_z = \frac{d_{\text{off}}}{2 \cos \theta_b} +\end{equation} + +#+begin_src matlab :exports none %% Tested bragg angles bragg = linspace(5, 80, 1000); % Bragg angle [deg] d_off = 10.5e-3; % Wanted offset between x-rays [m] #+end_src -#+begin_src matlab +#+begin_src matlab :exports none %% Vertical Jack motion as a function of Bragg angle dz = d_off./(2*cos(bragg*pi/180)); #+end_src +This relation is shown in Figure [[fig:jack_motion_bragg_angle]]. + #+begin_src matlab :exports none %% Jack motion as a function of Bragg angle figure; @@ -122,6 +134,8 @@ exportFig('figs/jack_motion_bragg_angle.pdf', 'width', 'wide', 'height', 'normal #+RESULTS: [[file:figs/jack_motion_bragg_angle.png]] +The required jack stroke is approximately 25mm. + #+begin_src matlab :results value replace :exports both %% Required Jack stroke ans = 1e3*(dz(end) - dz(1)) @@ -701,6 +715,148 @@ Here, we map the piezo forces at the center of stiffness. Let's first compute the Jacobian: +* Open-Loop Noise Budgeting +:PROPERTIES: +:header-args:matlab+: :tangle matlab/dcm_noise_budget.m +:END: +<> + +** Introduction :ignore: + +#+begin_src latex :file noise_budget_dcm_schematic_fast_jack_frame.pdf +\begin{tikzpicture} + % Blocs + \node[block] (G) {$G(s)$}; + \node[addb, left= of G] (adddu) {}; + \node[block, left= of adddu] (K) {$K(s)$}; + \node[addb={+}{}{}{}{-}, left= of K] (subL) {}; + \node[addb, right= of G] (addd) {}; + \node[addb, below right=0.8 and 0.6 of addd] (adddn) {}; + + % Connections and labels + \draw[->] (subL.east) -- (K.west); + \draw[->] (K.east) -- (adddu.west); + \draw[->] (adddu.east) -- (G.west); + \draw[->] (G.east) -- (addd.west); + \draw[->] (addd-|adddn)node[branch]{}node[above]{$y_{\text{fj}}$} -- (adddn.north); + \draw[->] (adddn.west) -| (subL.south) node[below right]{$y_{\text{fj},m}$}; + \draw[<-] (adddu.north) -- ++(0, 1) node[below right]{$d_u$}; + \draw[<-] (addd.north) -- ++(0, 1) node[below right]{$d_{\text{fj}}$}; + \draw[<-] (adddn.east) -- ++(1, 0)coordinate(dn) node[above left]{$n_{\text{fj}}$}; + \draw[->] (addd.east) -- (addd-|dn); +\end{tikzpicture} +#+end_src + +#+name: fig:noise_budget_dcm_schematic_fast_jack_frame +#+caption: Schematic representation of the control loop in the frame of one fast jack +#+RESULTS: +[[file:figs/noise_budget_dcm_schematic_fast_jack_frame.png]] + +** Matlab Init :noexport:ignore: +#+begin_src matlab +%% dcm_noise_budget.m +% Basic uniaxial noise budgeting +#+end_src + +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) +<> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes +<> +#+end_src + +#+begin_src matlab :tangle no :noweb yes +<> +#+end_src + +#+begin_src matlab :eval no :noweb yes +<> +#+end_src + +#+begin_src matlab :noweb yes +<> + +%% Frequency vector for noise budget [Hz] +f = logspace(-1, 3, 1000); +#+end_src + +** Power Spectral Density of signals + +Interferometer noise: +#+begin_src matlab +Wn = 6e-11*(1 + s/2/pi/200)/(1 + s/2/pi/60); % m/sqrt(Hz) +#+end_src + +#+begin_src matlab :results value replace :exports results :tangle no +sprintf('Measurement noise: %.2f [nm,rms]', 1e9*sqrt(trapz(f, abs(squeeze(freqresp(Wn, f, 'Hz'))).^2))); +#+end_src + +#+RESULTS: +: Measurement noise: 0.79 [nm,rms] + +DAC noise (amplified by the PI voltage amplifier, and converted to newtons): +#+begin_src matlab +Wdac = tf(3e-8); % V/sqrt(Hz) +Wu = Wdac*22.5*10; % N/sqrt(Hz) +#+end_src + +#+begin_src matlab :results value replace :exports results :tangle no +sprintf('DAC noise: %.2f [uV,rms]', 1e6*sqrt(trapz(f, abs(squeeze(freqresp(Wdac, f, 'Hz'))).^2))); +#+end_src + +#+RESULTS: +: DAC noise: 0.95 [uV,rms] + +Disturbances: +#+begin_src matlab +Wd = 5e-7/(1 + s/2/pi); % m/sqrt(Hz) +#+end_src + +#+begin_src matlab :results value replace :exports results :tangle no +sprintf('Disturbance motion: %.2f [um,rms]', 1e6*sqrt(trapz(f, abs(squeeze(freqresp(Wd, f, 'Hz'))).^2))); +#+end_src + +#+RESULTS: +: Disturbance motion: 0.61 [um,rms] + +#+begin_src matlab :exports none :tangle no +save('matlab/mat/asd_noises_disturbances.mat', 'Wn', 'Wu', 'Wd'); +#+end_src + +#+begin_src matlab :eval no +%% Save ASD of noise and disturbances +save('mat/asd_noises_disturbances.mat', 'Wn', 'Wu', 'Wd'); +#+end_src + +** Open Loop disturbance and measurement noise +The comparison of the amplitude spectral density of the measurement noise and of the jack parasitic motion is performed in Figure [[fig:open_loop_noise_budget_fast_jack]]. +It confirms that the sensor noise is low enough to measure the motion errors of the crystal. + +#+begin_src matlab :exports none +%% Bode plot for the plant (strain gauge output) +figure; +hold on; +plot(f, abs(squeeze(freqresp(Wn, f, 'Hz'))), ... + 'DisplayName', 'n'); +plot(f, abs(squeeze(freqresp(Wd, f, 'Hz'))), ... + 'DisplayName', 'd'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]'); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); +xlim([f(1), f(end)]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/open_loop_noise_budget_fast_jack.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:open_loop_noise_budget_fast_jack +#+caption: Open Loop noise budgeting +#+RESULTS: +[[file:figs/open_loop_noise_budget_fast_jack.png]] + * Active Damping Plant (Strain gauges) :PROPERTIES: :header-args:matlab+: :tangle matlab/dcm_active_damping_strain_gauges.m @@ -1673,6 +1829,95 @@ The HAC-IFF control strategy can effectively reduce the transmissibility of the This reduction is effective inside the bandwidth of the controller. #+end_important +** Close Loop noise budget +#+begin_src matlab :exports none +%% Load disturbances +load('asd_noises_disturbances.mat'); +#+end_src + +Let's compute the amplitude spectral density of the jack motion errors due to the sensor noise, the actuator noise and disturbances. + +#+begin_src matlab +%% Computation of ASD of contribution of inputs to the closed-loop motion +% Error due to disturbances +asd_d = abs(squeeze(freqresp(Wd*(1/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz'))); +% Error due to actuator noise +asd_u = abs(squeeze(freqresp(Wu*(G_dp(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz'))); +% Error due to sensor noise +asd_n = abs(squeeze(freqresp(Wn*(G_dp(1,1)*Khac(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz'))); +#+end_src + +The closed-loop ASD is then: +#+begin_src matlab +%% ASD of the closed-loop motion +asd_cl = sqrt(asd_d.^2 + asd_u.^2 + asd_n.^2); +#+end_src + +The obtained ASD are shown in Figure [[fig:close_loop_asd_noise_budget_hac_iff]]. + +#+begin_src matlab :exports none +%% Noise Budget (ASD) +f = logspace(-1, 3, 1000); + +figure; +hold on; +plot(f, asd_n, 'DisplayName', '$n$'); +plot(f, asd_u, 'DisplayName', '$d_u$'); +plot(f, asd_d, 'DisplayName', '$d$'); +plot(f, asd_cl, 'k--', 'DisplayName', '$y$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]'); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +xlim([f(1), f(end)]); +ylim([1e-16, 1e-8]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/close_loop_asd_noise_budget_hac_iff.pdf', 'width', 'wide', 'height', 'tall'); +#+end_src + +#+name: fig:close_loop_asd_noise_budget_hac_iff +#+caption: Closed Loop noise budget +#+RESULTS: +[[file:figs/close_loop_asd_noise_budget_hac_iff.png]] + +Let's compare the open-loop and close-loop cases (Figure [[fig:cps_comp_ol_cl_hac_iff]]). +#+begin_src matlab :exports none +% Amplitude spectral density of the open loop motion errors [m/sqrt(Hz)] +asd_ol = abs(squeeze(freqresp(Wd, f, 'Hz'))); +#+end_src + +#+begin_src matlab :exports none +% CPS of open-loop motion [m^2] +cps_ol = flip(-cumtrapz(flip(f), flip(asd_ol.^2))); +% CPS of closed-loop motion [m^2] +cps_cl = flip(-cumtrapz(flip(f), flip(asd_cl.^2))); +#+end_src + +#+begin_src matlab :exports none +%% Cumulative Power Spectrum - Motion error of fast jack +figure; +hold on; +plot(f, cps_ol, 'DisplayName', sprintf('OL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_ol(1)))); +plot(f, cps_cl, 'DisplayName', sprintf('CL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_cl(1)))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('CPS [$m^2$]'); +legend('location', 'southwest', 'FontSize', 8); +xlim([f(1), f(end)]); +% ylim([1e-16, 1e-8]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file replace +exportFig('figs/cps_comp_ol_cl_hac_iff.pdf', 'width', 'wide', 'height', 'normal'); +#+end_src + +#+name: fig:cps_comp_ol_cl_hac_iff +#+caption: Cumulative Power Spectrum of the open-loop and closed-loop motion error along one fast-jack +#+RESULTS: +[[file:figs/cps_comp_ol_cl_hac_iff.png]] + * Helping Functions :noexport: ** Initialize Path #+NAME: m-init-path diff --git a/dcm-simscape.pdf b/dcm-simscape.pdf index db2cdb2..9d29a54 100644 Binary files a/dcm-simscape.pdf and b/dcm-simscape.pdf differ diff --git a/figs/close_loop_asd_noise_budget_hac_iff.pdf b/figs/close_loop_asd_noise_budget_hac_iff.pdf new file mode 100644 index 0000000..6e87f02 Binary files /dev/null and b/figs/close_loop_asd_noise_budget_hac_iff.pdf differ diff --git a/figs/close_loop_asd_noise_budget_hac_iff.png b/figs/close_loop_asd_noise_budget_hac_iff.png new file mode 100644 index 0000000..8b1b62b Binary files /dev/null and b/figs/close_loop_asd_noise_budget_hac_iff.png differ diff --git a/figs/cps_comp_ol_cl_hac_iff.pdf b/figs/cps_comp_ol_cl_hac_iff.pdf new file mode 100644 index 0000000..6acecd1 --- /dev/null +++ b/figs/cps_comp_ol_cl_hac_iff.pdf @@ -0,0 +1,1241 @@ +%PDF-1.4 +% +1 0 obj +<< +/Producer (Apache FOP Version 2.4.0-SNAPSHOT: PDFDocumentGraphics2D) +/CreationDate (D:20211130174934+01'00') +>> +endobj +2 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +3 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +4 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +5 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +6 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +7 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +8 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +9 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +10 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +11 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +12 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +13 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +14 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +15 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +16 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +17 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +18 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +19 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +20 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +21 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +22 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +23 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +24 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +25 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +26 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +27 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +28 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +29 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +30 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +31 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +32 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +33 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +34 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +35 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +36 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +37 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +38 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +39 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +40 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +41 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +42 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +43 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +44 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +45 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +46 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +47 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +48 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +49 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +50 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +51 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +52 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +53 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +54 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +55 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +56 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +57 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +58 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +59 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +60 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +61 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +62 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +63 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +64 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +65 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +66 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +67 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +68 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +69 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +70 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +71 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +72 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +73 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +74 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +75 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +76 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +77 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +78 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +79 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +80 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +81 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +82 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +83 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +84 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +85 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +86 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +87 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +88 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +89 0 obj +<< +/Type /ExtGState +/CA 0.2509804 +>> +endobj +90 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +91 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +92 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +93 0 obj +<< +/Type /ExtGState +/CA 0.14901961 +>> +endobj +94 0 obj +<< /Length 95 0 R /Filter /FlateDecode >> +stream +x$=v; ˖8L |^ތL22Jw7\$6l}>-n)zhB_PR_ Oǿ_gy[ fFۿ?m|/-#~˱׽mgV߮!^^Cܶ[̵X5m=V˱kHװ[[b5x{X5k(J/[J5]_Cndz?v} m[<0t} nAN%5,]_Cخ/B;_W|"Kʽ[8H-wbE\)$y~7q]VXoQm?1" ˄d\.-5m[..SnmGo,⺼̡S8|" ̬Gk[:.1s y8GohT%fnJ,l!X.1 qGo,,q7aߏc%\egebE\:LRbE\U[_W)|"˚2,%XuyYK4aGo,⺼:Rb ﺼUJ,fCJ̟ҲmK,CZj"+lx$ڄℱqoL/N]/{8aIWߘ^pmں6uf~k2SC(8GGWWq]*Zב?O.|ۭ^}GWWq]O[߂fO]]U~Kz+XU\W}zN潯b*낾 +Im%%;V1}tuוA[ ՘JU]]umWôO>꠭b/Pj/ߏU]]ś3[EjϱWW R8JXUr3l0bLxSnnVu[c*ޔ3 +y;d*ޓ2:U'7cO,\vZb*ޓt[9dU'7.z!8U'7cDy|tu͘Duus1tuΘ-$j#GWcwθ,q:U';㞴~.]]{3I}GWW쌇ǟܚ\Q1U')L.{cGWW ɵCD͟\]{rR_r=9j* U]]{r2<`GW ޓngl*ޓ20r}ߋxON]^>dw3vBwi1tuI;GWW ̝|WOMY>7% ?[b*ޔ[9?GWW|t5+Mɹ”x +wU'9[8a'W}q2O=Y[[i;D5'3s1[m1ru IJ5'1S1; -{aޓi?^'Wn lv͟\]!+pL^"sI57JOu/%570Ř.ͷSğF~˿o[f ~b0g\c쟞9g!{t>riF8(|q?&.ɹ%߲_"l?]?[?Y?W?U?S?Q?O/4jBOʧzR(>;j>>Oʩ>>O*>>O>=>>OJ>>>>O +>> ‡gIu֧YaIMI9I%IaէgY?y?Y-u͕/(賳[ٜD"5gчg?59><%O-p~}sv}xSӛ3óݜ]A6g7Їg_D9gЇg?9><_ύrή~sv|x7O=qNNw~sv|xSó]=`:g7χg_{ꜜvq]i>W=xabz/R +ۄT<Įe*gbߧ?}i*y&l /(gR/c~E=Sd\Tɾ)tLԜ_L:]So]D-/fOersՃ2A{Ӵ/fϿe͒ ̐ql^?jbڥ-34zP^3|,3IS#zQĶvΛ}LʼyK.Y?~z6i QC싺eO?5~<햚t>Ξ;ӾR64=v6d7vW?u]u^o64%vvW?]Qΰo2'2a̿t]f-앹O}`m{i:`nK]_lzI_^ku7[-2sew^]v6W?]Oem-{i:Me"n;Kcl${is:-dTcvLn+ޑSy^LfKjgw^vOjg۟Yo\{. oM.7y>n+.ۀﶞ2kvͦfOjinku7^Yvzno0< e/~ ;n+ zγD짮ﶏ2/vƱW")k%f+j\,R\OMw^.OãSl`[}5<x'?'k~y¸lw9=M. ',i[(It8?o9? |Ҩ{8? ~8[GB+e{GC>q~0t~?8/)f]8.3`.,(~MJQWӾHK{1.Egr^LѓNCO:$_΋U?~*}^SпsfTb_biM7l) ~[/ٙ խcɯAjR.CuZT&U=BX^ߓݠW -ϻL@ZhEa@[_tEOPaq( ʭϒ.)Qm)ڛBںC Cj|}k[ +Š>S7AH\KjKzlg G0v }v1iU QzXal`AXv~I91MB>Glо4@<`AʞaA$I5k3HX ՋFe2C궕(DHE +6rAQS8/%Q:S`\ZQq}?$Jݨ(AP^<-8  UdU{ɱ2=:P`[hB2$s^llԞv=Ď}n)$Gl.DrL@IL0JٿoewTRޛ=[(G\qjgo%mFQYg8pI |T +_6GzyRfv];3IJ]:6Q;S}\߿H>d՛q[*FȕoP֭Wwג ʱGWrKҡ`HHI`[a ^͐^ ؛!:K`ˀ*JRzJzKlO *FuV$P$_K݆>d@-&Ȗ[>t\DFIIH:np(dPt+vbzz/v6HOmc6I A>H eՋz:qhd)tK줠.T;HAwU/Ԟs]8jAj}5S}}|fl$bW %Y]]`oEJͰAImׯd :!2O m@u۫ wLiQnN-؁Qӗ/I'-AU1W@=oph m߫PNw[z-}@uD[^kEp2J`Q>.iPAV0XMc(-oMԈ+} IͪI.; jYDdX"Hw xCi%(ɸǨ&I/e1۸w ~[Bį~ ?Wf{M"j:GKohDв :j+]ojQ:?ȱu*uwI0A-we )2P6єѷR>$%K~yc5TO *DƋKcOo. $%-C +, үIq}\$]>w3 6xX:@v'M񅣣HWkFbZ7=9Ihn/C +? =LtסuN&{w]IOBBPMUEIR?9tڷ1IO/7qM:۠0,!/M{ẗ́ޠӐ&`ߦÄFoN%euFI@ӓ9% ,6U#K*u9Knh:/k@etBYrd%2D8gT+p BbEQ((F]4{tr"6 KAu~0.}(=$ežG7 ]UIC_$.amTfLh 6˘5 +3&cH:]*ڡ(w@JlTCAmI:exR¢]teiM!tТ/1nCgk +^ +lXuSk by@W%'I;e#I<4ͫ]J6aڭK2ސO60^ؼz[$" oTVW}=fטћF8k%:uuo"!Q>=LwuHҧol~enLlTĮvwHfteٙl$2me|ʞ}]€[$xU++ 6CV/}@H[]dJwQ Ec@~(_f/)F52id7r*bK^I۲ A6ﱍ65|T1,FE_P.2)htQ7JmTi_nZ+qk o$u^Wtek[dPEoqX]OO6$PsC̶Pt1;&Hv[zү\eyHJ[$ XCB?.xIsw +1g%^ {\t}Tl$3(O -c<=ːcP2Pn~L;D<=iU[*}T)nX@=Iț0qUګ0 }1a~lNs8 bK e':uJa݈6"_C|eZou2p fq:rdHfOy  Db$`F񋥹$J*~GC֭HEW2 [1;뿻^݌Z,}]w=LWtx]fMl] Hra%mA;n'AIHYsaaKHtXI`'7CӨbQR2Cztqm6J# 鄳MoeH)p+HJvW0?#kh6 +mWֶ )5v@a+K 󽏻uɥT6V)B xuA캷3mdy;?<PNHjIHiQРxjDH"3m1_Leq%}/Bd#gzQYqU!8R/7S5]uaj~j9*[)Mׅ+^LpuSyx)n};%4jvΥ$n_')rOum~3!ul`-cա)]/^M!d +[g:IBO>O3qI7߿C75#4% ǿ<~^AqLEo[qbFW>F +fOQzRB!([ݱ}nkݰe.Lw6Yⵅp''aiDQ26H/=*U5N~E7:oFyl],͚SУl%#&.vW1oG/e&Ig_$A8u`2WJs׻N4SHs5덗m5f +9iCcF <: c}HCWDC3Y$<$:xrOZIu;Tk]hܻ`BT.{&HBXpVfBVvm-:!k@JHd&w_ fQSaJڑ;rCQ`2-@MIʅFᄐ)2pT] FRtuP =wYdzc4zd` D&` IdRb9V_@m6JlY;t I7Mw,-_Ux a6iG;Tb2b+B6+dx쌛%E)(._H^0ڸ'L:L0PGw*(`$Q*}m> + A7n@*'ut2(Ž`!!j*7"`iG"DnQJہtUX㭃Orq*v%I.ONG92#ԥJz=A@Xvh\u<[2S+ ~6#[*D\F]d1s"KkaL7sKiDts:δLPB_ʵp < -:Ie꿀0@ss[5}̳)d` GvP4/${)yy:?1䒮)>ui˘{<*1KmrOipX`/%4F */H%@r zpzo&GȠ=>~6 Mus5Nm 9 7Q|\ԹI_(|I1`"WaV.qFƄΥm +(U3%yƎKR0x#ǯ'R"K92vfZ%0i+$:8.uL`lBqy/ֆRIm* 3cB@|IIi,02wT]yKw,c_&N@J1IE"=J%`njd^(ddq#o8vVu,t߀N7i`fm#&fѦzJdI>bт/X{Xo vGo,P]l'\|;;^X +`S]%&:A}ȂFږXWPxC r5h$fLVDLi`LqZ2uכ;xeqٍ43M^āuvE/O&bI7 + h!qI(vE2G ĽBm#`'$=|Xeݿ9~i +xWTiLc{ڒ-BYJГN37 C=nٖ2uͣ7kaKRl"eQX'8}C/ML +ݳre ݇r'91{T@z6o%glu39bt.BarOCxt: +':^4$;ICZiLABAF:d`TȻa;u0]I7ϰ)>]g2q +Rv2BM. LR33pBEQ6#HaU~@ wOwu`3a02hH6 h{oٞ,)mU:"xL,P!1ֱR%U}k!"P[|Q.Is#Cz +]"{bajLȀI*ev7$#٢`~͢\_){7qB"":tEP]b-:% dɈJlBFL|\c{; +$%4q`4v["Q=0V)S0 vuDFo]W] LJt>Qg]wƳƄ & 8F"v >NWB$0uvv<+d^[ 't-j҆ssI0(:>NW5<|-J9.ו\dzG +|G<: wxu>drE\uYbn$HޖnKPb +y@$r߬)ìEƕe, Rt>QUi +M -ض4rnm5CΩml[[aV0$" y*E$/W- !t+.wd\Œ;aC}\B1`m]ٚyY ~soKxuWRe26_1H (.k!F-d#7!J5 ԛ0fsH澰Î" wpJ/2ǽ 0_a: &F!PGN #Q,5ȰåDr&WFr ccR#% Jq{Xe+x=<X ޕ/0Edެ&9 }ɣ^v%6޳_GBJsa`ġ1ahѐ'H+?##D +1]dQ`BO9IK13 |fOgͥ#W9אm_]yΫ} Ѱ}joB +ҵ2p{XL3u tbtԐHGxK4L:^=%^CljH]Ī YǵbJkĥ"s# +5gӵ/9z5zqPJqx ̃(_ .2FUb <0v+Ѣ Rȭ"DN6x,qJ<.hՇU: +F8>Uj=m?p1 wEzwb tS2&ެ@LX:эZAcE^^H+mgW[6*kQWa`5G!F/Q`%ؾÈl!FϣHxḪR[LK!|N@fZ2=A/^ #81 Y> L~c`@#')cF[ď-0],$J|v0*HWKeqj&)`YfluJHV0U!uPt<|R&z͆F V e 77ʟsGRn>%%Lg/EJ>g CAݻnl+1tJ0w/"ݗ+j>`^j kʁ/B# XW -3XC8܎FxpHa&GDHW'kXVkJ(YZ$ L.hP T5d[@oH4L[DL1EߔL|CwO"pi~BHQݪӝw!hAEѻaHHn}a:}2nɇů/ED|6 -|vڟTs1E&>]BJ)E $j+nɃ5"HDJmuCozM lqc%6b>LV#qg`,=TMD ۍ5]=@/9#¡CpmfdDZYmPUzUX:dZpB2"+9#[! +VoTF [5yBծ%ɰH@+HJR%..wȣ*0dl}`yqu ]!~,bR(s1Li`X^>YhA۶:`v8AYW+f 129z5 /n`UZPr)x,]Sw {؎YQ5`dwDCԖ1.!#9@)h8Ȝ>?Nړ.#~06s;WWn!{'ůNC-z\1 u +}Xr&=",._~%՘th>`1Y2rm CF2`S#,/8}:,ߨFuC|^Ay~D! K#m_!N#s.yNRCƜ.D<Ks5 ^uBI8&AȤpW;#dw}!g3̓O)PFzwtX))`}raސjN1wnma9xbn 𺵣w(Q_hc^E=cҏ!z~Ǭ)e*N"`G5,yL`|8#1M\"\h/eV* Dg;K0nB'D8 />SE.;S^Jve7%{H 1(jI,fm>,6`dF+4B*Ѯ"?!mI1qT'O@e09g#T-sT"Da_em;}\!9'`Nu_EF^a/O3XwbN_=Lq3M%)nݓvW~{< $́9H\ù* S|$ȡ4Q̹kl)GVF'zt.¥&|ykw+> +ң^s.9dod DŻaEsꨇ-qi4pf ^"NƸi_+Gi +#5|-TB%IoY+,X*a$DXKC + +$D0]hhJF?P`{f $uNmt|`t( #F EiadoT +m #%v"Ee| ЎͲV:S 7av zEmWنqQv͹ ?>šf6SaDE}>",ng]Q KFao$I{lX5‘&Ј"tGtN{12pl# I7\IoxhB+ d;mI^qh)QK;Hew0nkujxQN +a}Ps:bIDpFd|G:n`͓h[`ǑPϤv+ =q3gA,:kct wQ y`qDsi]y28-QL dYߑ6Us{s,ɉfc4L[)u db4*Jԧn40"wLΣ)mzJ}!!/&I%~QLѲ ۇǝ2n RP&#&>XoBӊIgH]c: hkTe*Fv< qDŽ1}3FK[ [ڝۑnp/Jӯh%fmJa٣% *kz1:]ּu;C]D'1^`w1ܽ055o ~ VQ [ +9he+V8-vM.E8Qe+7>gQ?ں):ڬ L!ʬ6|THRa;vI>d0^af4lX[:H'q$xaiH*WdksDk`4; 6GtCIr$J5 +ybwn%GȁeedN5Xڥ% &"<[3בD=KǙ+h&( 2$wKͪ #} T#J>,iYAig(uƈ!ADͭG+VߡY*կ/5iuFd0QFH 0?k;h8^e}6t{*ǒaRb|%aX*-\X%;Ul>7FQGO$i>Ҷ%i6#ƯK"0I>LԦ;4Ү[mi#7Z3Եk$.Hn{=p0\⁑G֥avUE Qu/t"Q +.=0ڟŸ.| Rʁ2-N &4I0C]Nn5_ñ]ς<T:/\Ѽ+_`֪2Fv׫<;;dmt}2[fϑ8 RNIO7 Cy=MĦ&'vB80Cx`InqG.~17k¸fa5BD0S`x-m(f0֊.f/w ay`^φuGto4F?E$āI>|ptIAF(Ò }`(.zQi_FQ4Jt$\?c-6byՃu>|\?%̴4u0qk z8Xce\߄<ޕqk=ΦP1XobG#b'R*DX[4kB{booFbAt4v B# ,_M`QjF+:u۔h X G/F~2H~[/``NKTыX":rp7FIΞ .Bna(apm3Pӑ0$轢mS96:/x{3KRt4k .Qق5k?Xޮk7K;ٯk 8kxdd73;d|*HՊdͫe=KN.n&o (nk7k1mqOM5'O%H1~*EazRŔXߤ?L-n4G0Bzǵtَ:('),Ƶ`4y2~Y[@5v-Y0xܞb/ /2گmcx؈/D^sr/T^; ׁtf:48f:׉)y (rȉ`r)l48Rv7R+U_X>z H͚hlwu@6_'bJvf:GDu`3׀i ׁ- '~/y_o5 ]bS|A6q|Pd p{: 'ҘY"IV&*ZFVBu`xa .d_6U.l_éDu`405 L_68 i._Du`5|`w/h g֯U ׁ5O\x9+vM_<{Z;Jh50!Ɓ{ +B669 sac`1pʻl`doYL7+ H˰l`)N5 5l`w&y +l`^𽒁 {Bнla{W:y26T06 (_V7-`sr :696UWVBl@69D 60z>: 6:jb ݙ˞ yxWf{X^񴐃 *( FvsX]60ΜMa44lbHP =3Gc6 -WB6#9FO=;QFB6 VMLa;20H?v +XW}plakI3]X9:}|aT&°}ư)ΰmPΤa3aERl 1ߝ8l =:gca@3wc09EL600kbX-hsLWw60;;t ƯA̱r hIEl`G +B#X uN;cǎ<DIuMtbY5R؝Ol`2P̱7(5{XΛ9+qziЃT̑fXk{Pw̙ȖNOA+fPǗ19f=T,ؽqB-ؑ2P9DH?Q Q<;cC^,3cpUNb{7,bm^ փg/ c훦7s9S.,cE0lsu@w1g̱|m.Dc,D јCtLDcdјCoP}I5q9fm18QL7hE9%yKp1;8A6Q9p(96qFy3.c޿+cV'2e;wU昕;y׃!\ leA3Mteqݍ`+s}g:XXf]X3:S9dΎ6q9F`&He82Îc+mcT;YL\X0|iMee1]\e`4^59#4f2Q:$v?f3Ê]  꽖6yxB`fu30(  '?3f3 ޚ9 0﫷rѕ̰tdƮfZ(̀{a,s03/ R`03[a-3a|0/^_9s&;Zfm03]9Fnf `0+F:a4v l7Ӄafw~,fujp{ H_pZ̰Lkh)9̊eS+a+oa1ṵgO,f` oJ3FwK0JL]/4fA`8{Mvﰶp3t:bFm,fu3 Y̊x.ŬP;z~,fO][X0D3d.,f`w.,f[ +4f`V}1 aƌnu1CS̵ aLl3X,Df`;ߚa. Lq m 9{fy~!2`gNI6"Ӱ̊~cJef.b3x2v +uˍ)LfyDF:v`230:S6%< 6eiߡOJJffJ6Νk&33LW230)u7\Y NџTrP Y1 ,Y&23}ל m"32223tϱX =̬jep,lf`\>fw؃ 5xp2-lf`fFQ8=Cd!3ҟ'df`;;OdfFmaW230I۠\̀ ٝc!3 ' 5Q!dLfVl4HUăAfjn!33=k#? άlf`v|f3+&&q@=әqX̀;Lgfᚍ]lfG% l7&_̀yBJgvu}tfZy21 Agf +e3,^̊ ?JgFު+N %!Y1=ҙ?k^W:30bqBgֻ3l34Pu8&:b +Y>љ jpi|f@;_gc|fVf3gF*9+gVdgFI ̀28/lf$kUf630Y]̬<? u2 ̬8pAgF;w|f`e wݟ: 3Y4-.>30g>3gJ( gF񸼓gkh'V>3SԱϬXͿ55l3Kp{#2l&4\͈Voaff 2Q1TB3L՛ Te7VF30,ms͌fϰ #Xك ,h%+Fua4}3/=͜}ah ISe>3шDgf8M8>ʅ $8W*3Z+)t}poTfZ+Nm̺;Wa3Xq•ʬXӇB+X'30ɇA۷ռ=xNiY<,؃ȍ*P&"32LvxΉ Y*ΩBia31HDf`y+^Ldf#؃Ȍ^잝*ja2e|W&30`H1lf2^>l&2MDa3Nq yИUsz*qŌ>FPk!1GLbfUر% A86q^ r!*,$f`:BbN \Iv q1~<,]V3Klub17oN+i:]bF5XŌyFka13Ula1o/Xcm_; |YIvk2H*W3ڒS+be%1DxB+َAI}Čc)3z˳GW3.ޱ|%1{q$f5ʃm&1" '3:P}+0ћ$f.ZЃÌo$f3\aڻ-fl]']8̜9, L7l0 ep2Ì.ϬahpqM څRt@a MfFZLw؝,Xr4H̬ ā1ф z!1 Ts~ K|a13BVZn!bfMo6 +bfdķ S9WbfD6Q0:`/,fdEê nNu򽒘Y/݃lm"1sWJATv'11;{(^ȞvÎEWO$fFѵ q)Č>A9c7'3 #~;~r=x̀胜g3&%t$z5%"u~lj؞Lbf|GJbf^q9,Sl"130dJbf-Jbf(n.$fF8kQf纙I̜p5SY&3'% Y0řL:35], vyS$f֐) bL6ћvg1fMh/Xpf16h6g30sa19Q3gw&3k/ɛ,,f`c$f(\X̌Uχag@w3]g,ftw}M,fF+,fƏUbf͡{e1#wY̌twOa3DϽwe1# -,fz>f| ZYf3+M)0ԱŌQ`3:DƭU-4f®AcfM]ZhP'3lmBcFk%23O#],ft:r%O,fK$fa. 5 Č`"uԅČdVΥpDG!sfoG}0 vz+50sotar-f{%1CSgwJbF[?-ۭOA F4N$f/'=EKE̞ںr.+2υYI's!1p. -xiHz`3hil&1`>$fQ}"1rYI̢k6+~'3 /6a9 '3:7wHPzwWcH=%Db[F^ Bbe.>p?8IOf8Nz+N5>ÝUl&1ÑO Y%oXO`/\(*u;X`f?LY3Rae0kS5Z l0l͙u +3 +\b57Of¬ioŒ=?SqAAu0#]OfqҜ)(ժuw0\/[(̦a/~f0 +kb`hl`ּʷsa0+ԤT4=Xʙ¬#phe0O fa,鱏pњQG?0V +nBzDaVHs/aXOf֙xJNf4(Pt +3a˭fV,Z  Mf;5\#r0½La3YFg~PQ}9 éVLolGr%1+,}kg3(}}&1x;zHܽ^Jbb= W!i](8#{0#ua_?sxh.rgr۠sl0 a͋NfdTo6s}Uf4hŠa َKިX6qAx2' +3HB|0K7K\YUρ>Q-qYhvb0+N'3$Ѯ03ks7,I9-fܵYdž2_a'3j0ȥ؃̲Fń ;Y=]=8&2svk2_&v P ~/kmAm6їA[WL_ OeP8oB_+M'kOeh{7r3{Y.ͳOeVZvOyxb/by5D#o%/Ü>x˪mǃl!/K "5T/%g2X3yZ%4[lk/T VD2=|?ф`;ї'lڑ.}&0?yAg36D`FaIOD`&U,P':H^P^gY=QEs۾=۴mŢ<NH8~5tû(C>];ȯu'k7ގ?К- B]!6o9tUY_.n+< ׏&cxqpgXphb1J=[DA;׏[de3s}]g^nf9Kl_O+شc!Öd_>ld4- +endstream +endobj +95 0 obj +26091 +endobj +96 0 obj +[94 0 R] +endobj +97 0 obj +<< + /Resources 98 0 R + /Type /Page + /MediaBox [0 0 312 179] + /CropBox [0 0 312 179] + /BleedBox [0 0 312 179] + /TrimBox [0 0 312 179] + /Parent 99 0 R + /Contents 96 0 R +>> +endobj +100 0 obj +<< + /Type /FontDescriptor + /FontName /EAAAAA+mwb_cmmi10 + /FontBBox [16 -215 882 715] + /Flags 33 + /CapHeight 0 + /Ascent 715 + /Descent -215 + /ItalicAngle 0 + /StemV 0 + /MissingWidth 500 + /FontFile2 101 0 R + /CIDSet 102 0 R +>> +endobj +101 0 obj +<< + /Length1 2208 + /Length 103 0 R + /Filter /FlateDecode +>> +stream +xUklURZ +tR;nyhyuQJ;nBPV@C|!!CC! 1FC`"l=36AEwwι; u S Y7w; Hs}On$gw:9QC~p} P̢( +sw>'ؕ~xʵX,>WtWO߽#,}җL R + Y~osRI !E-Lj6eŝid~-c8wq 0 $Ѓn kVA2,Xp*'Q!ݖI'?_;-xA/w"gaP3Ck~NF1b#A뼆/\%\,q*( 7{)lwb 1_5ļ+}$7(Z:,.*,qHs :j Y;kNEr(rPSt|Da8E꧔bS!)}l0TE\▞4+}0/|jljl[̕.c5+ƅ׶}vd]|վ BW6<՘{vm2Rx=\ U|qJrw^Gߩ&O';pVTl$=-#t)pNfhtN 2>N'h۩.K(#tx+1߁sEY6d1ۛ EY,7Ʉ\F( 3299Yc6 g@cfb#$N#!<~4o_2U+*Y q«g+Y(i۶,ֈl)[3H4G4r79WV&jb@Ybbƞ J>ɾHt@ Γ bqyOnSQ#Wr5,%)?mt :٪-^r5Kz5FڈMNlS" &~έ]DOZ\|O L,Ֆ{Ncۚ't~ZuL~)Եɴڸ"pf8sDs0/TTX|՛B!p!_p +endstream +endobj +103 0 obj +1522 +endobj +102 0 obj +<< /Length 104 0 R /Filter /FlateDecode >> +stream +xk`` +endstream +endobj +104 0 obj +11 +endobj +105 0 obj +<< + /Type /Font + /Subtype /Type0 + /BaseFont /EAAAAA+mwb_cmmi10 + /Encoding /Identity-H + /ToUnicode 106 0 R + /DescendantFonts [107 0 R] +>> +endobj +107 0 obj +<< /Type /Font +/BaseFont /EAAAAA+mwb_cmmi10 +/CIDToGIDMap /Identity +/Subtype /CIDFontType2 +/CIDSystemInfo << /Registry (Adobe) /Ordering (UCS) /Supplement 0 >> +/FontDescriptor 100 0 R +/DW 0 +/W [ 0 [750 404 ] ] +>> +endobj +106 0 obj +<< /Length 108 0 R /Filter /FlateDecode >> +stream +x]Pj0 +t;()i?FqJH?FK )X V#NWut b%xؖsm2k<#D\M #n8H7EW5#&\ڮY"Bø3`pJ#)?auZ?jyFleTg$M`3^r:^*s1Ģ } +endstream +endobj +108 0 obj +234 +endobj +109 0 obj +<< + /Type /FontDescriptor + /FontName /EAAAAB+mwa_cmmi10 + /FontBBox [-34 -250 1047 750] + /Flags 33 + /CapHeight 683 + /Ascent 750 + /Descent -250 + /ItalicAngle 0 + /StemV 0 + /MissingWidth 500 + /FontFile2 110 0 R + /CIDSet 111 0 R +>> +endobj +110 0 obj +<< + /Length1 2636 + /Length 112 0 R + /Filter /FlateDecode +>> +stream +xV}L[?}c iHc?`l`VQlfcH%[DQ(ʶ[UUiUʲ[mZeUط}Bʦꦥ[{Lm׼w{޻ O 'jm/;\΢¼0W USewxd:ynhcRGEmA>?7]Jdzc1咔t6 eaVJy:yL:;}ZFgbiKz pX.}#k"NJ{p +/Kp WspS)Q8 B7/tB swؿov3e~ɬ0t~~~E3t5HRMJ:BBwpn#pހaבW`^s|AeyY +<\\p@N$2#|}#f(7JEBmX7ՉU8ualEɯN%"`ɿvTþs#2X B7€2݀ 3A$D7Ѱk#a0 @\%& vELR4TjyhjlanGGm{͹硪2Vb@ +~Jqad"n .-U d"VɅJ?!$A-+P0ɃZD A,bB,kr؅x~ n)"M ,*Zd"ذhXG|; ut8=; rWgT=_3 MYC¿ISaVԬ乵Fl0QB?xy3noqCqleSV`6Q{p[˵zx5 +(b2MVjCdvqŴ G-t-{9TG{EϻrcGO t{NwN :=ɫ^^,*^[aXoh{}LjtPW]LWX)jS}!'ggxמjkɦ5-:/b.)ihxlH,;3ǫ'tX'ۯ7^9m,f}]a15}T:4qoBy +k嵌NJꚡCV܍*CW女%J {Rw[UYb/6Ǻ6kReN*7V?_2e5r9 X~ X53E,^k2EkK.rb4}Eߦ_N]ڱsőۃӡo> +stream +xk`&F +endstream +endobj +113 0 obj +14 +endobj +114 0 obj +<< + /Type /Font + /Subtype /Type0 + /BaseFont /EAAAAB+mwa_cmmi10 + /Encoding /Identity-H + /ToUnicode 115 0 R + /DescendantFonts [116 0 R] +>> +endobj +116 0 obj +<< /Type /Font +/BaseFont /EAAAAB+mwa_cmmi10 +/CIDToGIDMap /Identity +/Subtype /CIDFontType2 +/CIDSystemInfo << /Registry (Adobe) /Ordering (UCS) /Supplement 0 >> +/FontDescriptor 109 0 R +/DW 0 +/W [ 0 [365 877 520 ] ] +>> +endobj +115 0 obj +<< /Length 117 0 R /Filter /FlateDecode >> +stream +x]Pj0 +t=,N%PR +9l[[N m琿*cah❼1N~!0h]Qՠ;[M2"u8uxn, >gIϠi$F8}~pB:ͯh2 AϥӉdzI?jՖAys +I[K&_7`ԷmR5Qo;wsV Qʫ9ux8U|~ +endstream +endobj +117 0 obj +241 +endobj +118 0 obj +<< + /Type /FontDescriptor + /FontName /EAAAAC+mwb_cmsy10 + /FontBBox [11 -215 942 727] + /Flags 33 + /CapHeight 0 + /Ascent 727 + /Descent -215 + /ItalicAngle 0 + /StemV 0 + /MissingWidth 500 + /FontFile2 119 0 R + /CIDSet 120 0 R +>> +endobj +119 0 obj +<< + /Length1 1992 + /Length 121 0 R + /Filter /FlateDecode +>> +stream +xUmle=Ҏ{`3^wLz׭s%["AZC[خueAM/CD%:_r4!HBb  m >s/^:jxiZ |'T~lTvvSdg҆70$\qm#U7s3lxl~b{gUg +l;"9I=g,Q!_^~uK_2 +y'.zWJYN,1qPo~eev2EcQw)8fg33 uk5Uuen(H",͍!UYl3&񆞄Mzmb'LHU3vUYy1VMHi=iD%S˺\> 5'- JnML1DIO&^S|r]O>Rl͹-6Wl0ȹAсQMU%EƉRXɤ0Yk4+R%nH+0EYL2&'kQU􀪜EX􊰨$-p&L"aMXTTao BUrIU̺vua7YKn-&#*ĸ9 Ony}-4h"'\YxkC%B5nń`9U;[ֽq_.|[_ޮٰT]ɚ` +U;͞6c%Z!_FΈghyF'ʌ +hR}Bfp'ޥ.Kd km ѽwBxc[EwqrSaw0/;`l>pJv.BLXG uLr0*`-,`l^sMtFS 4ioh:F֠SO KxzGI;MJ$El-Z (;Iˎ9DHu<&*+qdlU;Aԡ٩L*=-m InEb&+ -R@feVbFј*@gKӇQ]'Igt&?H{U5JQ#TG*;Ixtld> +stream +xk +endstream +endobj +122 0 obj +9 +endobj +123 0 obj +<< + /Type /Font + /Subtype /Type0 + /BaseFont /EAAAAC+mwb_cmsy10 + /Encoding /Identity-H + /ToUnicode 124 0 R + /DescendantFonts [125 0 R] +>> +endobj +125 0 obj +<< /Type /Font +/BaseFont /EAAAAC+mwb_cmsy10 +/CIDToGIDMap /Identity +/Subtype /CIDFontType2 +/CIDSystemInfo << /Registry (Adobe) /Ordering (UCS) /Supplement 0 >> +/FontDescriptor 118 0 R +/DW 0 +/W [ 0 [750 776 ] ] +>> +endobj +124 0 obj +<< /Length 126 0 R /Filter /FlateDecode >> +stream +x]Pj0+l CԒX}q z0h[lE^X gBpjV& +:G:g} +endstream +endobj +126 0 obj +234 +endobj +127 0 obj +<< + /Type /FontDescriptor + /FontName /EAAAAD+mwa_cmr10 + /FontBBox [-43 -250 1008 750] + /Flags 33 + /CapHeight 683 + /Ascent 750 + /Descent -250 + /ItalicAngle 0 + /StemV 0 + /MissingWidth 500 + /FontFile2 128 0 R + /CIDSet 129 0 R +>> +endobj +128 0 obj +<< + /Length1 7252 + /Length 130 0 R + /Filter /FlateDecode +>> +stream +xY tSי۴Z.bI%y, ƖM`l ;B@C +LSNB -MPBrҔ6N929iO6ߓ6wy] A!vC[Adcc#H w&)Տy|~=80{iqֈ@$tq؏`?kh|f~kRn ޏ]D$qkxk|9191=so7h?7Eۢn)v=Cn»68 810C  P +>$ľ^c#)v'Ȋ1b:JߥOQTe2]r {8Ÿw7:\p^"\ <Gq8 `ayA~x^JT*^ըUDE\&E%_P!Kh弄lo2Hs[#RwC."T"=.2"[ct.ZLu--\Ƹ +I5"BZ(~l%CR|UZ +`O ҉8r >?Pj\9(y݈@y ]1t_ߔ])$"%2'Ie5LU1END؍PDv8Нl*)+.% 18GDVKH[CXI +Gk8:֤B`''}R ڢ`ݽ[?` %Er+ lkݘFGBpfJU.3(bY3( +>nꅃL+6f2EV0" HHmSO/Om{qAㆪ@i^NT(gą&-/:NRHy,M%JQE6[^E>RN{(*浢,fB!N]+yZ<;7I+`_mq%㿯%%Xli9';F7RJW]h=ﹴ߱lʳ(S3,TVJFS4*D$K-#mYjқ%^PgDل c2xQ*ɗYSx{Kf4jd隞ݥe\@PzH:6\atW^*0.ew*%b9%ެ(&$m$p1b>O0۫*xMKZPk[a"hI9:c-- %FE0uF38x,/%kPwGyT=Q*F*IJV[P2Xbxm}k}>ִ֣ʃjڱiVlov;|_,u"\j|K' +ҍ 82ҨDfKAmNN/% 5& +͍1Ǽbi|夔u +BL!ޜ6y-OG]d".[?ǰ>P1y=U}~,ekXN3TEhŪG%$&(1AJ1tq0︔<èK"R[*ƸmU*Dc:Nnο_۪?nٺ*d>:]Kv S؍) Z>>@1g6k/(H2f^6콽΁,˝?X"#h(EhUT&MMYV@mir4,)E{^@1TALάRIPz.'" +*}<>fXi SR+%z2n,P7UdK18k穿tpY9PIjτ&j{Wي ٩"(PR əndE M DIkT&EѽO~(4B,/W0/[$oN)\,Iu$B. +5r>) uwXZFE Ykyq)T-_,>sk̭X2 +>~4M9ߊb?/&Sz -QJ0 !Iz=J4#Ӓ+ye +;V$>Qe7M2 f+stqŠsXr4&Hӏg\Q 5zA,1ߧw2yM*׫(4:?=eyP|G|*C\f +ȥ<> X H0MJEJ/-S 4)DOo|i[c!{Դ T`޹WGh=nǙ>s>lZ|w#QEaޓM='Vꪞ4ywM|9%1ç/)+5ƯEW"P"pşIcH n5ÆaX=@ h7-I#mbڬ HX̮^a\dVމxeheL%hM 4MV/N/'~tm|~9<'9Ҫ_U07jJJn{\R;כ lj7夢ju}Ɂ-=|oq܁1L'T82(QEP:' YNdexXEb(;60ݙޙ^u觾<Ddپ|kT S|tc2tOGQ:PVlר6_*I!i$bDW07(8Mf"I,SCd] aP 0bQ_h +6^Jvj^;_,mcĉLg>[3[3B ΁l:2_ItRm͊$U.ڵ-vGeㆠrփ9V/^G}%]Ԙŧ]V}VCY1z5^G""؍/ kF{D_[|,2TwغcGwz^,+u~dӏBԿ;7 ΉNe"CC͕_?isY-=ӔA"uJbQ$o*UD#BO>W./UOv&Z+ IX_8Hc#^$jTayII#eMV[a4X1 .[*>} yTŀ/p,+aᆃVGeƹMd[,3xv?7_MMm{${C#0η8֟fNFmӹ~)gnzvXs Sd + k0@aQ2'&͞al{3ԛٕXW 7+-Ch,"|*&~~58ܽ#bK =b.|YT&ҏWaN}q99.,p[CL$GHqpa.xyY& )4 J2oiȵ8q%a$ػ")P8M4i<YH!i߄JIXm0 03l*!BD~(BkD Ӹf :FǑ1ᗌ!Ya,#ݦ^?x,~-Y;7xG5I8·*'& +s~3g1c#l?ݿm/ꮛ6LlʼnʉޙቭN1W9 G sV$/ lG~qu#$K +ܿdm%A(l!^yŖ7] .9N7/ 62ʼn}"A4",Ȯf7ov$xtفt̙ +endstream +endobj +130 0 obj +5316 +endobj +129 0 obj +<< /Length 131 0 R /Filter /FlateDecode >> +stream +xk`Ȳ(q/< +endstream +endobj +131 0 obj +20 +endobj +132 0 obj +<< + /Type /Font + /Subtype /Type0 + /BaseFont /EAAAAD+mwa_cmr10 + /Encoding /Identity-H + /ToUnicode 133 0 R + /DescendantFonts [134 0 R] +>> +endobj +134 0 obj +<< /Type /Font +/BaseFont /EAAAAD+mwa_cmr10 +/CIDToGIDMap /Identity +/Subtype /CIDFontType2 +/CIDSystemInfo << /Registry (Adobe) /Ordering (UCS) /Supplement 0 >> +/FontDescriptor 127 0 R +/DW 0 +/W [ 0 [365 500 500 500 500 651 391 443 526 555 555 443 526 276 750 443 276 500 500 500 722 680 555 776 625 276 776 833 394 500 ] ] +>> +endobj +133 0 obj +<< /Length 135 0 R /Filter /FlateDecode >> +stream +x]j0E +-EG%E8Բ@~xgkT=(GC?9I61)e'&yL۳*5٨d,/f}T}CLQ9EN~]uҰGFgr}UtĒgW)/s {E07ڜS.e/qip{JӜ2P +*{>!K1%PP@*Pـ_?Z(E B@> +endobj +136 0 obj +<< + /Type /Catalog + /Pages 99 0 R + /Lang (x-unknown) +>> +endobj +98 0 obj +<< + /Font << + /F953 105 0 R + /F949 114 0 R + /F950 123 0 R + /F951 132 0 R +>> + /ProcSet [/PDF /ImageB /ImageC /Text] + /ExtGState << + /GS1 2 0 R + /GS2 3 0 R + /GS3 4 0 R + /GS4 5 0 R + /GS5 6 0 R + /GS6 7 0 R + /GS7 8 0 R + /GS8 9 0 R + /GS9 10 0 R + /GS10 11 0 R + /GS11 12 0 R + /GS12 13 0 R + /GS13 14 0 R + /GS14 15 0 R + /GS15 16 0 R + /GS16 17 0 R + /GS17 18 0 R + /GS18 19 0 R + /GS19 20 0 R + /GS20 21 0 R + /GS21 22 0 R + /GS22 23 0 R + /GS23 24 0 R + /GS24 25 0 R + /GS25 26 0 R + /GS26 27 0 R + /GS27 28 0 R + /GS28 29 0 R + /GS29 30 0 R + /GS30 31 0 R + /GS31 32 0 R + /GS32 33 0 R + /GS33 34 0 R + /GS34 35 0 R + /GS35 36 0 R + /GS36 37 0 R + /GS37 38 0 R + /GS38 39 0 R + /GS39 40 0 R + /GS40 41 0 R + /GS41 42 0 R + /GS42 43 0 R + /GS43 44 0 R + /GS44 45 0 R + /GS45 46 0 R + /GS46 47 0 R + /GS47 48 0 R + /GS48 49 0 R + /GS49 50 0 R + /GS50 51 0 R + /GS51 52 0 R + /GS52 53 0 R + /GS53 54 0 R + /GS54 55 0 R + /GS55 56 0 R + /GS56 57 0 R + /GS57 58 0 R + /GS58 59 0 R + /GS59 60 0 R + /GS60 61 0 R + /GS61 62 0 R + /GS62 63 0 R + /GS63 64 0 R + /GS64 65 0 R + /GS65 66 0 R + /GS66 67 0 R + /GS67 68 0 R + /GS68 69 0 R + /GS69 70 0 R + /GS70 71 0 R + /GS71 72 0 R + /GS72 73 0 R + /GS73 74 0 R + /GS74 75 0 R + /GS75 76 0 R + /GS76 77 0 R + /GS77 78 0 R + /GS78 79 0 R + /GS79 80 0 R + /GS80 81 0 R + /GS81 82 0 R + /GS82 83 0 R + /GS83 84 0 R + /GS84 85 0 R + /GS85 86 0 R + /GS86 87 0 R + /GS87 88 0 R + /GS88 89 0 R + /GS89 90 0 R + /GS90 91 0 R + /GS91 92 0 R + /GS92 93 0 R +>> +>> +endobj +xref +0 137 +0000000000 65535 f +0000000015 00000 n +0000000145 00000 n +0000000197 00000 n +0000000249 00000 n +0000000301 00000 n +0000000353 00000 n +0000000405 00000 n +0000000457 00000 n +0000000509 00000 n +0000000561 00000 n +0000000614 00000 n +0000000667 00000 n +0000000720 00000 n +0000000773 00000 n +0000000826 00000 n +0000000879 00000 n +0000000932 00000 n +0000000985 00000 n +0000001038 00000 n +0000001091 00000 n +0000001144 00000 n +0000001197 00000 n +0000001250 00000 n +0000001303 00000 n +0000001356 00000 n +0000001409 00000 n +0000001462 00000 n +0000001515 00000 n +0000001568 00000 n +0000001621 00000 n +0000001674 00000 n +0000001727 00000 n +0000001780 00000 n +0000001833 00000 n +0000001887 00000 n +0000001941 00000 n +0000001995 00000 n +0000002049 00000 n +0000002103 00000 n +0000002156 00000 n +0000002209 00000 n +0000002262 00000 n +0000002315 00000 n +0000002368 00000 n +0000002421 00000 n +0000002474 00000 n +0000002527 00000 n +0000002580 00000 n +0000002633 00000 n +0000002686 00000 n +0000002739 00000 n +0000002792 00000 n +0000002845 00000 n +0000002898 00000 n +0000002951 00000 n +0000003004 00000 n +0000003057 00000 n +0000003110 00000 n +0000003163 00000 n +0000003216 00000 n +0000003269 00000 n +0000003322 00000 n +0000003375 00000 n +0000003428 00000 n +0000003481 00000 n +0000003534 00000 n +0000003587 00000 n +0000003640 00000 n +0000003693 00000 n +0000003746 00000 n +0000003799 00000 n +0000003852 00000 n +0000003905 00000 n +0000003958 00000 n +0000004011 00000 n +0000004064 00000 n +0000004117 00000 n +0000004170 00000 n +0000004223 00000 n +0000004276 00000 n +0000004329 00000 n +0000004382 00000 n +0000004435 00000 n +0000004488 00000 n +0000004541 00000 n +0000004594 00000 n +0000004647 00000 n +0000004700 00000 n +0000004753 00000 n +0000004806 00000 n +0000004860 00000 n +0000004914 00000 n +0000004968 00000 n +0000005022 00000 n +0000031189 00000 n +0000031211 00000 n +0000031236 00000 n +0000046707 00000 n +0000046571 00000 n +0000031430 00000 n +0000031682 00000 n +0000033324 00000 n +0000033302 00000 n +0000033413 00000 n +0000033433 00000 n +0000033824 00000 n +0000033593 00000 n +0000034136 00000 n +0000034157 00000 n +0000034413 00000 n +0000036424 00000 n +0000036402 00000 n +0000036516 00000 n +0000036536 00000 n +0000036931 00000 n +0000036696 00000 n +0000037250 00000 n +0000037271 00000 n +0000037523 00000 n +0000038967 00000 n +0000038945 00000 n +0000039054 00000 n +0000039073 00000 n +0000039464 00000 n +0000039233 00000 n +0000039776 00000 n +0000039797 00000 n +0000040052 00000 n +0000045488 00000 n +0000045466 00000 n +0000045586 00000 n +0000045606 00000 n +0000046107 00000 n +0000045765 00000 n +0000046550 00000 n +0000046631 00000 n +trailer +<< + /Root 136 0 R + /Info 1 0 R + /ID [<5C54364111754CE99DC509BA4D78A028> <5C54364111754CE99DC509BA4D78A028>] + /Size 137 +>> +startxref +48229 +%%EOF diff --git a/figs/cps_comp_ol_cl_hac_iff.png b/figs/cps_comp_ol_cl_hac_iff.png new file mode 100644 index 0000000..7e637e7 Binary files /dev/null and b/figs/cps_comp_ol_cl_hac_iff.png differ diff --git a/figs/noise_budget_dcm_schematic_fast_jack_frame.pdf b/figs/noise_budget_dcm_schematic_fast_jack_frame.pdf new file mode 100644 index 0000000..9e0723e Binary files /dev/null and b/figs/noise_budget_dcm_schematic_fast_jack_frame.pdf differ diff --git a/figs/noise_budget_dcm_schematic_fast_jack_frame.png b/figs/noise_budget_dcm_schematic_fast_jack_frame.png new file mode 100644 index 0000000..6517cb3 Binary files /dev/null and b/figs/noise_budget_dcm_schematic_fast_jack_frame.png differ diff --git a/figs/noise_budget_dcm_schematic_fast_jack_frame.svg b/figs/noise_budget_dcm_schematic_fast_jack_frame.svg new file mode 100644 index 0000000..f9ca59a --- /dev/null +++ b/figs/noise_budget_dcm_schematic_fast_jack_frame.svg @@ -0,0 +1,211 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/figs/open_loop_noise_budget_fast_jack.pdf b/figs/open_loop_noise_budget_fast_jack.pdf new file mode 100644 index 0000000..5e6282b Binary files /dev/null and b/figs/open_loop_noise_budget_fast_jack.pdf differ diff --git a/figs/open_loop_noise_budget_fast_jack.png b/figs/open_loop_noise_budget_fast_jack.png new file mode 100644 index 0000000..fa84f9d Binary files /dev/null and b/figs/open_loop_noise_budget_fast_jack.png differ diff --git a/matlab/dcm_active_damping_iff.m b/matlab/dcm_active_damping_iff.m index 55b8309..7334cc9 100644 --- a/matlab/dcm_active_damping_iff.m +++ b/matlab/dcm_active_damping_iff.m @@ -54,10 +54,8 @@ G_fs.OutputName = {'fs_ur', 'fs_uh', 'fs_d'}; -% #+RESULTS: -% | -1.4113e-13 | 1.0339e-13 | 3.774e-14 | -% | 1.0339e-13 | -1.4113e-13 | 3.774e-14 | -% | 3.7792e-14 | 3.7792e-14 | -7.5585e-14 | +% The Bode plot of the identified dynamics is shown in Figure [[fig:iff_plant_bode_plot]]. +% At high frequency, the diagonal terms are constants while the off-diagonal terms have some roll-off. %% Bode plot for the plant @@ -82,7 +80,7 @@ for i = 1:2 end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +ylabel('Amplitude'); set(gca, 'XTickLabel',[]); legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2); ylim([1e-13, 1e-7]); @@ -102,7 +100,11 @@ linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); % Controller - Root Locus +% We want to have integral action around the resonances of the system, but we do not want to integrate at low frequency. +% Therefore, we can use a low pass filter. + +%% Integral Force Feedback Controller Kiff_g1 = eye(3)*1/(1 + s/2/pi/20); %% Root Locus for IFF @@ -141,10 +143,19 @@ legend('location', 'northwest'); % [[file:figs/iff_root_locus.png]] -%% Integral Force Feedback Controller +%% Integral Force Feedback Controller with optimal gain Kiff = g*Kiff_g1; -% Damped Plant +%% Save the IFF controller +save('mat/Kiff.mat', 'Kiff'); + + + +% #+name: fig:schematic_jacobian_frame_fastjack_iff +% #+caption: Use of Jacobian matrices to obtain the system in the frame of the fastjacks +% #+RESULTS: +% [[file:figs/schematic_jacobian_frame_fastjack_iff.png]] + %% Input/Output definition clear io; io_i = 1; @@ -157,8 +168,8 @@ io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1; % Force Sensor {3x1} [m] io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1; -%% DCM Kinematics -load('mat/dcm_kinematics.mat'); +%% Load DCM Kinematics +load('dcm_kinematics.mat'); %% Identification of the Open Loop plant controller.type = 0; % Open Loop @@ -172,6 +183,12 @@ G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io); G_dp.InputName = {'u_ur', 'u_uh', 'u_d'}; G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'}; + + +% The dynamics from $\bm{u}$ to $\bm{d}_{\text{fj}}$ (open-loop dynamics) and from $\bm{u}^\prime$ to $\bm{d}_{\text{fs}}$ are compared in Figure [[fig:comp_damped_undamped_plant_iff_bode_plot]]. +% It is clear that the Integral Force Feedback control strategy is very effective in damping the resonances of the plant. + + %% Comparison of the damped and undamped plant figure; tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); @@ -221,5 +238,3 @@ ylim([-180, 0]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); - -save('mat/Kiff.mat', 'Kiff'); diff --git a/matlab/dcm_active_damping_strain_gauges.m b/matlab/dcm_active_damping_strain_gauges.m index 996d100..8247ac8 100644 --- a/matlab/dcm_active_damping_strain_gauges.m +++ b/matlab/dcm_active_damping_strain_gauges.m @@ -40,13 +40,11 @@ clear io; io_i = 1; %% Inputs % Control Input {3x1} [N] -io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; -% % Stepper Displacement {3x1} [m] -% io(io_i) = linio([mdl, '/d'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/control_system'], 1, 'openinput'); io_i = io_i + 1; %% Outputs % Strain Gauges {3x1} [m] -io(io_i) = linio([mdl, '/sg'], 1, 'openoutput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/DCM'], 2, 'openoutput'); io_i = io_i + 1; %% Extraction of the dynamics G_sg = linearize(mdl, io); @@ -57,12 +55,12 @@ G_sg.OutputName = {'sg_ur', 'sg_uh', 'sg_d'}; % #+RESULTS: -% | -1.4113e-13 | 1.0339e-13 | 3.774e-14 | -% | 1.0339e-13 | -1.4113e-13 | 3.774e-14 | -% | 3.7792e-14 | 3.7792e-14 | -7.5585e-14 | +% | 4.4443e-09 | 1.0339e-13 | 3.774e-14 | +% | 1.0339e-13 | 4.4443e-09 | 3.774e-14 | +% | 3.7792e-14 | 3.7792e-14 | 4.4444e-09 | -%% Bode plot for the plant +%% Bode plot for the plant (strain gauge output) figure; tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); @@ -83,7 +81,8 @@ end hold off; set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +ylim([1e-14, 1e-7]); ax2 = nexttile; hold on; @@ -95,7 +94,133 @@ set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); hold off; yticks(-360:90:360); -ylim([-180, 180]); +ylim([-180, 0]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% Relative Active Damping + +Krad_g1 = eye(3)*s/(s^2/(2*pi*500)^2 + 2*s/(2*pi*500) + 1); + + + +% As can be seen in Figure [[fig:relative_damping_root_locus]], very little damping can be added using relative damping strategy using strain gauges. + + +%% Root Locus for IFF +gains = logspace(3, 8, 200); + +figure; + +hold on; +plot(real(pole(G_sg)), imag(pole(G_sg)), 'x', 'color', colors(1,:), ... + 'DisplayName', '$g = 0$'); +plot(real(tzero(G_sg)), imag(tzero(G_sg)), 'o', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); + +for g = gains + clpoles = pole(feedback(G_sg, g*Krad_g1, -1)); + plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ... + 'HandleVisibility', 'off'); +end + +% Optimal gain +g = 2e5; +clpoles = pole(feedback(G_sg, g*Krad_g1, -1)); +plot(real(clpoles), imag(clpoles), 'x', 'color', colors(2,:), ... + 'DisplayName', sprintf('$g=%.0e$', g)); +hold off; +xlim([-6, 0]); ylim([0, 2700]); +xlabel('Real Part'); ylabel('Imaginary Part'); +legend('location', 'northwest'); + + + +% #+name: fig:relative_damping_root_locus +% #+caption: Root Locus for the relative damping control +% #+RESULTS: +% [[file:figs/relative_damping_root_locus.png]] + + +Krad = -g*Krad_g1; + +% Damped Plant +% The controller is implemented on Simscape, and the damped plant is identified. + + +%% Input/Output definition +clear io; io_i = 1; + +%% Inputs +% Control Input {3x1} [N] +io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1; + +%% Outputs +% Force Sensor {3x1} [m] +io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1; + +%% DCM Kinematics +load('dcm_kinematics.mat'); + +%% Identification of the Open Loop plant +controller.type = 0; % Open Loop +G_ol = J_a_111*inv(J_s_111)*linearize(mdl, io); +G_ol.InputName = {'u_ur', 'u_uh', 'u_d'}; +G_ol.OutputName = {'d_ur', 'd_uh', 'd_d'}; + +%% Identification of the damped plant with Relative Active Damping +controller.type = 2; % RAD +G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io); +G_dp.InputName = {'u_ur', 'u_uh', 'u_d'}; +G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'}; + +%% Comparison of the damped and undamped plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_ol(1,1), freqs, 'Hz'))), ... + 'DisplayName', 'd - OL'); +plot(freqs, abs(squeeze(freqresp(G_ol(2,2), freqs, 'Hz'))), ... + 'DisplayName', 'uh - OL'); +plot(freqs, abs(squeeze(freqresp(G_ol(3,3), freqs, 'Hz'))), ... + 'DisplayName', 'ur - OL'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--', ... + 'DisplayName', 'd - IFF'); +plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--', ... + 'DisplayName', 'uh - IFF'); +plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--', ... + 'DisplayName', 'ur - IFF'); +for i = 1:2 + for j = i+1:3 + plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +ylim([1e-12, 1e-6]); + +ax2 = nexttile; +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(1,1), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(2,2), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(G_ol(3,3), freqs, 'Hz')))); +set(gca,'ColorOrderIndex',1) +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '--'); +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '--'); +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '--'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 0]); linkaxes([ax1,ax2],'x'); xlim([freqs(1), freqs(end)]); diff --git a/matlab/dcm_hac_iff.m b/matlab/dcm_hac_iff.m index 4e5cb27..894a884 100644 --- a/matlab/dcm_hac_iff.m +++ b/matlab/dcm_hac_iff.m @@ -32,3 +32,308 @@ colors = colororder; %% Frequency Vector freqs = logspace(1, 3, 1000); + +% System Identification +% Let's identify the damped plant. + + +%% Input/Output definition +clear io; io_i = 1; + +%% Inputs +% Control Input {3x1} [N] +io(io_i) = linio([mdl, '/control_system'], 1, 'input'); io_i = io_i + 1; + +%% Outputs +% Force Sensor {3x1} [m] +io(io_i) = linio([mdl, '/DCM'], 1, 'openoutput'); io_i = io_i + 1; + +%% Load DCM Kinematics and IFF controller +load('dcm_kinematics.mat'); +load('Kiff.mat'); + +%% Identification of the damped plant with IFF +controller.type = 1; % IFF +G_dp = J_a_111*inv(J_s_111)*linearize(mdl, io); +G_dp.InputName = {'u_ur', 'u_uh', 'u_d'}; +G_dp.OutputName = {'d_ur', 'd_uh', 'd_d'}; + +%% Comparison of the damped and undamped plant +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', 'd - IFF'); +plot(freqs, abs(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-', ... + 'DisplayName', 'uh - IFF'); +plot(freqs, abs(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-', ... + 'DisplayName', 'ur - IFF'); +for i = 1:2 + for j = i+1:3 + plot(freqs, abs(squeeze(freqresp(G_dp(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +ylim([1e-12, 1e-8]); + +ax2 = nexttile; +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(1,1), freqs, 'Hz'))), '-'); +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(2,2), freqs, 'Hz'))), '-'); +plot(freqs, 180/pi*angle(squeeze(freqresp(G_dp(3,3), freqs, 'Hz'))), '-'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 0]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + +% High Authority Controller +% Let's design a controller with a bandwidth of 100Hz. +% As the plant is well decoupled and well approximated by a constant at low frequency, the high authority controller can easily be designed with SISO loop shaping. + + +%% Controller design +wc = 2*pi*100; % Wanted crossover frequency [rad/s] +a = 2; % Lead parameter + +Khac = diag(1./diag(abs(evalfr(G_dp, 1j*wc)))) * ... % Diagonal controller + wc/s * ... % Integrator + 1/(sqrt(a))*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))) * ... % Lead + 1/(s^2/(4*wc)^2 + 2*s/(4*wc) + 1); % Low pass filter + +%% Save the HAC controller +save('mat/Khac_iff.mat', 'Khac'); + +%% Loop Gain +L_hac_lac = G_dp * Khac; + +%% Bode Plot of the Loop Gain +figure; +tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); + +ax1 = nexttile([2,1]); +hold on; +plot(freqs, abs(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-', ... + 'DisplayName', 'd'); +plot(freqs, abs(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-', ... + 'DisplayName', 'uh'); +plot(freqs, abs(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-', ... + 'DisplayName', 'ur'); +for i = 1:2 + for j = i+1:3 + plot(freqs, abs(squeeze(freqresp(L_hac_lac(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); +ylim([1e-2, 1e1]); + +ax2 = nexttile; +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(1,1), freqs, 'Hz'))), '-'); +plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(2,2), freqs, 'Hz'))), '-'); +plot(freqs, 180/pi*angle(squeeze(freqresp(L_hac_lac(3,3), freqs, 'Hz'))), '-'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); +hold off; +yticks(-360:90:360); +ylim([-180, 0]); + +linkaxes([ax1,ax2],'x'); +xlim([freqs(1), freqs(end)]); + + + +% #+name: fig:hac_iff_loop_gain_bode_plot +% #+caption: Bode Plot of the Loop gain for the High Authority Controller +% #+RESULTS: +% [[file:figs/hac_iff_loop_gain_bode_plot.png]] + + +%% Compute the Eigenvalues of the loop gain +Ldet = zeros(3, length(freqs)); + +Lmimo = squeeze(freqresp(L_hac_lac, freqs, 'Hz')); +for i_f = 1:length(freqs) + Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); +end + + + +% As shown in the Root Locus plot in Figure [[fig:loci_hac_iff_fast_jack]], the closed loop system should be stable. + + +%% Plot of the eigenvalues of L in the complex plane +figure; +hold on; +% Angle used to draw the circles +theta = linspace(0, 2*pi, 100); +% Unit circle +plot(cos(theta), sin(theta), '--'); +% Circle for module margin +plot(-1 + min(min(abs(Ldet + 1)))*cos(theta), min(min(abs(Ldet + 1)))*sin(theta), '--'); + +for i = 1:3 + plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), 'k.'); + plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), 'k.'); +end +% Unstable Point +plot(-1, 0, 'kx', 'HandleVisibility', 'off'); +hold off; +set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); +xlabel('Real'); ylabel('Imag'); +axis square; +xlim([-3, 1]); ylim([-2, 2]); + +% Performances +% In order to estimate the performances of the HAC-IFF control strategy, the transfer function from motion errors of the stepper motors to the motion error of the crystal is identified both in open loop and with the HAC-IFF strategy. + + +%% Input/Output definition +clear io; io_i = 1; + +%% Inputs +% Jack Motion Erros {3x1} [m] +io(io_i) = linio([mdl, '/stepper_errors'], 1, 'input'); io_i = io_i + 1; + +%% Outputs +% Interferometer Output {3x1} [m] +io(io_i) = linio([mdl, '/DCM'], 1, 'output'); io_i = io_i + 1; + +%% Identification of the transmissibility of errors in open-loop +controller.type = 0; % Open Loop +T_ol = inv(J_s_111)*linearize(mdl, io)*J_a_111; +T_ol.InputName = {'e_dz', 'e_ry', 'e_rx'}; +T_ol.OutputName = {'dx', 'ry', 'rx'}; + +%% Load DCM Kinematics and IFF controller +load('dcm_kinematics.mat'); +load('Kiff.mat'); + +%% Identification of the transmissibility of errors with HAC-IFF +controller.type = 3; % IFF +T_hl = inv(J_s_111)*linearize(mdl, io)*J_a_111; +T_hl.InputName = {'e_dz', 'e_ry', 'e_rx'}; +T_hl.OutputName = {'dx', 'ry', 'rx'}; + + + +% #+RESULTS: +% : 1 + +% And both transmissibilities are compared in Figure [[fig:stepper_transmissibility_comp_ol_hac_iff]]. + + +%% Transmissibility of stepper errors +f = logspace(0, 3, 1000); + +figure; +hold on; +plot(f, abs(squeeze(freqresp(T_ol(1,1), f, 'Hz'))), '-', ... + 'DisplayName', '$d_z$ - OL'); +plot(f, abs(squeeze(freqresp(T_ol(2,2), f, 'Hz'))), '-', ... + 'DisplayName', '$r_y$ - OL'); +plot(f, abs(squeeze(freqresp(T_ol(3,3), f, 'Hz'))), '-', ... + 'DisplayName', '$r_x$ - OL'); +set(gca,'ColorOrderIndex',1) +plot(f, abs(squeeze(freqresp(T_hl(1,1), f, 'Hz'))), '--', ... + 'DisplayName', '$d_z$ - HAC-IFF'); +plot(f, abs(squeeze(freqresp(T_hl(2,2), f, 'Hz'))), '--', ... + 'DisplayName', '$r_y$ - HAC-IFF'); +plot(f, abs(squeeze(freqresp(T_hl(3,3), f, 'Hz'))), '--', ... + 'DisplayName', '$r_x$ - HAC-IFF'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Stepper transmissibility'); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +ylim([1e-2, 1e2]); +xlim([f(1), f(end)]); + +% Close Loop noise budget + +%% Load disturbances +load('asd_noises_disturbances.mat'); + + + +% Let's compute the amplitude spectral density of the jack motion errors due to the sensor noise, the actuator noise and disturbances. + + +%% Computation of ASD of contribution of inputs to the closed-loop motion +% Error due to disturbances +asd_d = abs(squeeze(freqresp(Wd*(1/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz'))); +% Error due to actuator noise +asd_u = abs(squeeze(freqresp(Wu*(G_dp(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz'))); +% Error due to sensor noise +asd_n = abs(squeeze(freqresp(Wn*(G_dp(1,1)*Khac(1,1)/(1 + G_dp(1,1)*Khac(1,1))), f, 'Hz'))); + + + +% The closed-loop ASD is then: + +%% ASD of the closed-loop motion +asd_cl = sqrt(asd_d.^2 + asd_u.^2 + asd_n.^2); + + + +% The obtained ASD are shown in Figure [[fig:close_loop_asd_noise_budget_hac_iff]]. + + +%% Noise Budget (ASD) +f = logspace(-1, 3, 1000); + +figure; +hold on; +plot(f, asd_n, 'DisplayName', '$n$'); +plot(f, asd_u, 'DisplayName', '$d_u$'); +plot(f, asd_d, 'DisplayName', '$d$'); +plot(f, asd_cl, 'k--', 'DisplayName', '$y$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]'); +legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +xlim([f(1), f(end)]); +ylim([1e-16, 1e-8]); + + + +% #+name: fig:close_loop_asd_noise_budget_hac_iff +% #+caption: Closed Loop noise budget +% #+RESULTS: +% [[file:figs/close_loop_asd_noise_budget_hac_iff.png]] + +% Let's compare the open-loop and close-loop cases (Figure [[fig:cps_comp_ol_cl_hac_iff]]). + +% Amplitude spectral density of the open loop motion errors [m/sqrt(Hz)] +asd_ol = abs(squeeze(freqresp(Wd, f, 'Hz'))); + +% CPS of open-loop motion [m^2] +cps_ol = flip(-cumtrapz(flip(f), flip(asd_ol.^2))); +% CPS of closed-loop motion [m^2] +cps_cl = flip(-cumtrapz(flip(f), flip(asd_cl.^2))); + +%% Cumulative Power Spectrum - Motion error of fast jack +figure; +hold on; +plot(f, cps_ol, 'DisplayName', sprintf('OL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_ol(1)))); +plot(f, cps_cl, 'DisplayName', sprintf('CL, $\\epsilon_d = %.0f$ [nm,rms]', 1e9*sqrt(cps_cl(1)))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('CPS [$m^2$]'); +legend('location', 'southwest', 'FontSize', 8); +xlim([f(1), f(end)]); +% ylim([1e-16, 1e-8]); diff --git a/matlab/dcm_identification.m b/matlab/dcm_identification.m index 2237ec7..47b85ee 100644 --- a/matlab/dcm_identification.m +++ b/matlab/dcm_identification.m @@ -63,7 +63,7 @@ G.OutputName = {'int_111_1', 'int_111_2', 'int_111_3'}; % Plant in the frame of the fastjacks -load('mat/dcm_kinematics.mat'); +load('dcm_kinematics.mat'); diff --git a/matlab/dcm_kinematics.m b/matlab/dcm_kinematics.m index d84c4d2..b6a771b 100644 --- a/matlab/dcm_kinematics.m +++ b/matlab/dcm_kinematics.m @@ -22,6 +22,15 @@ colors = colororder; freqs = logspace(1, 3, 1000); % Bragg Angle +% There is a simple relation eqref:eq:bragg_angle_formula between: +% - $d_{\text{off}}$ is the wanted offset between the incident x-ray and the output x-ray +% - $\theta_b$ is the bragg angle +% - $d_z$ is the corresponding distance between the first and second crystals + +% \begin{equation} \label{eq:bragg_angle_formula} +% d_z = \frac{d_{\text{off}}}{2 \cos \theta_b} +% \end{equation} + %% Tested bragg angles bragg = linspace(5, 80, 1000); % Bragg angle [deg] @@ -30,6 +39,11 @@ d_off = 10.5e-3; % Wanted offset between x-rays [m] %% Vertical Jack motion as a function of Bragg angle dz = d_off./(2*cos(bragg*pi/180)); + + +% This relation is shown in Figure [[fig:jack_motion_bragg_angle]]. + + %% Jack motion as a function of Bragg angle figure; plot(bragg, 1e3*dz) @@ -42,6 +56,8 @@ xlabel('Bragg angle [deg]'); ylabel('Jack Motion [mm]'); % #+RESULTS: % [[file:figs/jack_motion_bragg_angle.png]] +% The required jack stroke is approximately 25mm. + %% Required Jack stroke ans = 1e3*(dz(end) - dz(1)) diff --git a/matlab/dcm_noise_budget.m b/matlab/dcm_noise_budget.m new file mode 100644 index 0000000..2f6167e --- /dev/null +++ b/matlab/dcm_noise_budget.m @@ -0,0 +1,71 @@ +% Matlab Init :noexport:ignore: + +%% dcm_noise_budget.m +% Basic uniaxial noise budgeting + +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +%% Path for functions, data and scripts +addpath('./mat/'); % Path for data + +%% Simscape Model - Nano Hexapod +addpath('./STEPS/') + +%% Colors for the figures +colors = colororder; + +%% Frequency Vector +freqs = logspace(1, 3, 1000); + +%% Frequency vector for noise budget [Hz] +f = logspace(-1, 3, 1000); + +% Power Spectral Density of signals + +% Interferometer noise: + +Wn = 6e-11*(1 + s/2/pi/200)/(1 + s/2/pi/60); % m/sqrt(Hz) + + + +% #+RESULTS: +% : Measurement noise: 0.79 [nm,rms] + +% DAC noise (amplified by the PI voltage amplifier, and converted to newtons): + +Wdac = tf(3e-8); % V/sqrt(Hz) +Wu = Wdac*22.5*10; % N/sqrt(Hz) + + + +% #+RESULTS: +% : DAC noise: 0.95 [uV,rms] + +% Disturbances: + +Wd = 5e-7/(1 + s/2/pi); % m/sqrt(Hz) + +%% Save ASD of noise and disturbances +save('mat/asd_noises_disturbances.mat', 'Wn', 'Wu', 'Wd'); + +% Open Loop disturbance and measurement noise +% The comparison of the amplitude spectral density of the measurement noise and of the jack parasitic motion is performed in Figure [[fig:open_loop_noise_budget_fast_jack]]. +% It confirms that the sensor noise is low enough to measure the motion errors of the crystal. + + +%% Bode plot for the plant (strain gauge output) +figure; +hold on; +plot(f, abs(squeeze(freqresp(Wn, f, 'Hz'))), ... + 'DisplayName', 'n'); +plot(f, abs(squeeze(freqresp(Wd, f, 'Hz'))), ... + 'DisplayName', 'd'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('ASD [$m/\sqrt{Hz}$]'); +legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); +xlim([f(1), f(end)]); diff --git a/matlab/mat/Khac_iff.mat b/matlab/mat/Khac_iff.mat new file mode 100644 index 0000000..afe9cb2 Binary files /dev/null and b/matlab/mat/Khac_iff.mat differ diff --git a/matlab/mat/asd_noises_disturbances.mat b/matlab/mat/asd_noises_disturbances.mat new file mode 100644 index 0000000..48e4926 Binary files /dev/null and b/matlab/mat/asd_noises_disturbances.mat differ