Update CSS
This commit is contained in:
174
matlab/matlab/sensor_description.m
Normal file
174
matlab/matlab/sensor_description.m
Normal file
@@ -0,0 +1,174 @@
|
||||
%% Clear Workspace and Close figures
|
||||
clear; close all; clc;
|
||||
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
addpath('src');
|
||||
freqs = logspace(0, 4, 1000);
|
||||
|
||||
% Sensor Dynamics
|
||||
% <<sec:sensor_dynamics>>
|
||||
% Let's consider two sensors measuring the velocity of an object.
|
||||
|
||||
% The first sensor is an accelerometer.
|
||||
% Its nominal dynamics $\hat{G}_1(s)$ is defined below.
|
||||
|
||||
m_acc = 0.01; % Inertial Mass [kg]
|
||||
c_acc = 5; % Damping [N/(m/s)]
|
||||
k_acc = 1e5; % Stiffness [N/m]
|
||||
g_acc = 1e5; % Gain [V/m]
|
||||
|
||||
G1 = g_acc*m_acc*s/(m_acc*s^2 + c_acc*s + k_acc); % Accelerometer Plant [V/(m/s)]
|
||||
|
||||
|
||||
|
||||
% The second sensor is a displacement sensor, its nominal dynamics $\hat{G}_2(s)$ is defined below.
|
||||
|
||||
w_pos = 2*pi*2e3; % Measurement Banwdith [rad/s]
|
||||
g_pos = 1e4; % Gain [V/m]
|
||||
|
||||
G2 = g_pos/s/(1 + s/w_pos); % Position Sensor Plant [V/(m/s)]
|
||||
|
||||
|
||||
|
||||
% These nominal dynamics are also taken as the model of the sensor dynamics.
|
||||
% The true sensor dynamics has some uncertainty associated to it and described in section [[sec:sensor_uncertainty]].
|
||||
|
||||
% Both sensor dynamics in $[\frac{V}{m/s}]$ are shown in Figure [[fig:sensors_nominal_dynamics]].
|
||||
|
||||
figure;
|
||||
% Magnitude
|
||||
ax1 = subplot(2,1,1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), '-', 'DisplayName', '$G_1(j\omega)$');
|
||||
plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), '-', 'DisplayName', '$G_2(j\omega)$');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude $\left[\frac{V}{m/s}\right]$'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'northeast', 'FontSize', 8);
|
||||
hold off;
|
||||
|
||||
% Phase
|
||||
ax2 = subplot(2,1,2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), '-');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), '-');
|
||||
set(gca,'xscale','log');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 180]);
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
% Sensor Model Uncertainty
|
||||
% <<sec:sensor_uncertainty>>
|
||||
% The uncertainty on the sensor dynamics is described by multiplicative uncertainty (Figure [[fig:sensor_model_noise_uncertainty]]).
|
||||
|
||||
% The true sensor dynamics $G_i(s)$ is then described by eqref:eq:sensor_dynamics_uncertainty.
|
||||
|
||||
% \begin{equation}
|
||||
% G_i(s) = \hat{G}_i(s) \left( 1 + W_i(s) \Delta_i(s) \right); \quad |\Delta_i(j\omega)| < 1 \forall \omega \label{eq:sensor_dynamics_uncertainty}
|
||||
% \end{equation}
|
||||
|
||||
% The weights $W_i(s)$ representing the dynamical uncertainty are defined below and their magnitude is shown in Figure [[fig:sensors_uncertainty_weights]].
|
||||
|
||||
W1 = createWeight('n', 2, 'w0', 2*pi*3, 'G0', 2, 'G1', 0.1, 'Gc', 1) * ...
|
||||
createWeight('n', 2, 'w0', 2*pi*1e3, 'G0', 1, 'G1', 4/0.1, 'Gc', 1/0.1);
|
||||
|
||||
W2 = createWeight('n', 2, 'w0', 2*pi*1e2, 'G0', 0.05, 'G1', 4, 'Gc', 1);
|
||||
|
||||
|
||||
|
||||
% The bode plot of the sensors nominal dynamics as well as their defined dynamical spread are shown in Figure [[fig:sensors_nominal_dynamics_and_uncertainty]].
|
||||
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(W1, freqs, 'Hz'))), 'DisplayName', '$|W_1(j\omega)|$');
|
||||
plot(freqs, abs(squeeze(freqresp(W2, freqs, 'Hz'))), 'DisplayName', '$|W_2(j\omega)|$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
ylim([0, 5]);
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
legend('location', 'northwest', 'FontSize', 8);
|
||||
|
||||
|
||||
|
||||
% #+name: fig:sensors_uncertainty_weights
|
||||
% #+caption: Magnitude of the multiplicative uncertainty weights $|W_i(j\omega)|$
|
||||
% #+RESULTS:
|
||||
% [[file:figs/sensors_uncertainty_weights.png]]
|
||||
|
||||
|
||||
figure;
|
||||
% Magnitude
|
||||
ax1 = subplot(2,1,1);
|
||||
hold on;
|
||||
plotMagUncertainty(W1, freqs, 'G', G1, 'color_i', 1, 'DisplayName', '$G_1$');
|
||||
plotMagUncertainty(W2, freqs, 'G', G2, 'color_i', 2, 'DisplayName', '$G_2$');
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
|
||||
plot(freqs, abs(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
set(gca, 'XTickLabel',[]);
|
||||
ylabel('Magnitude $[\frac{V}{m/s}]$');
|
||||
ylim([1e-2, 2e3]);
|
||||
legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2);
|
||||
hold off;
|
||||
ylim([1e-2, 1e4])
|
||||
|
||||
% Phase
|
||||
ax2 = subplot(2,1,2);
|
||||
hold on;
|
||||
plotPhaseUncertainty(W1, freqs, 'G', G1, 'color_i', 1);
|
||||
plotPhaseUncertainty(W2, freqs, 'G', G2, 'color_i', 2);
|
||||
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G1, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_1$');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G2, freqs, 'Hz'))), 'DisplayName', '$\hat{G}_2$');
|
||||
set(gca,'xscale','log');
|
||||
yticks(-180:90:180);
|
||||
ylim([-180 180]);
|
||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
||||
hold off;
|
||||
linkaxes([ax1,ax2],'x');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
|
||||
% Sensor Noise
|
||||
% <<sec:sensor_noise>>
|
||||
% The noise of the sensors $n_i$ are modelled by shaping a white noise with unitary PSD $\tilde{n}_i$ eqref:eq:unitary_noise_psd with a LTI transfer function $N_i(s)$ (Figure [[fig:sensor_model_noise_uncertainty]]).
|
||||
% \begin{equation}
|
||||
% \Phi_{\tilde{n}_i}(\omega) = 1 \label{eq:unitary_noise_psd}
|
||||
% \end{equation}
|
||||
|
||||
% The Power Spectral Density of the sensor noise $\Phi_{n_i}(\omega)$ is then computed using eqref:eq:sensor_noise_shaping and expressed in $[\frac{(m/s)^2}{Hz}]$.
|
||||
% \begin{equation}
|
||||
% \Phi_{n_i}(\omega) = \left| N_i(j\omega) \right|^2 \Phi_{\tilde{n}_i}(\omega) \label{eq:sensor_noise_shaping}
|
||||
% \end{equation}
|
||||
|
||||
% The weights $N_1$ and $N_2$ representing the amplitude spectral density of the sensor noises are defined below and shown in Figure [[fig:sensors_noise]].
|
||||
|
||||
omegac = 0.15*2*pi; G0 = 1e-1; Ginf = 1e-6;
|
||||
N1 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
|
||||
|
||||
omegac = 1000*2*pi; G0 = 1e-6; Ginf = 1e-3;
|
||||
N2 = (Ginf*s/omegac + G0)/(s/omegac + 1)/(1 + s/2/pi/1e4);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(N1, freqs, 'Hz'))), '-', 'DisplayName', '$|N_1(j\omega)|$');
|
||||
plot(freqs, abs(squeeze(freqresp(N2, freqs, 'Hz'))), '-', 'DisplayName', '$|N_2(j\omega)|$');
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('ASD $\left[ \frac{m/s}{\sqrt{Hz}} \right]$');
|
||||
hold off;
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
legend('location', 'northeast', 'FontSize', 8);
|
||||
|
||||
% Save Model
|
||||
% All the dynamical systems representing the sensors are saved for further use.
|
||||
|
||||
|
||||
save('./mat/model.mat', 'freqs', 'G1', 'G2', 'N2', 'N1', 'W2', 'W1');
|
||||
Reference in New Issue
Block a user