tdehaeze/sensor-fusion-test-bench
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

new identification data

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-08-28 17:15:21 +02:00
parent a59c0b2007
commit 5e9c385767
9 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
mat/identification_chirp_01_1000_iff.mat Normal file
View File

Binary file not shown.

BIN
mat/identification_chirp_02_2000_iff.mat Normal file
View File

Binary file not shown.

BIN
mat/identification_chirp_40_400_bis.mat Normal file
View File

Binary file not shown.

BIN
mat/identification_noise.mat Normal file
View File

Binary file not shown.

BIN
mat/identification_noise_bis.mat Normal file
View File

Binary file not shown.

2
runtest.m
View File

@ -17,7 +17,7 @@ u = data(:, 6);
f_meas = data(:, 7);
t = data(:, 8);
save('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
save('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
%%
d = detrend(d, 0);

16
sensor_fusion_analysis.m
View File

@ -88,7 +88,7 @@ title('Huddle Test')
legend();
%% Compare Theoretical model with identified one
id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
id_ol = load('./mat/identification_noise_bis.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
% Detrend Data
id_ol.d = detrend(id_ol.d, 0);
@ -117,7 +117,7 @@ hold off;
ax2 = subplot(2, 1, 2);
hold on;
plot(f, 180/pi*unwrap(angle(tf_fmeas_est)), '-')
plot(f, 180/pi*angle(tf_fmeas_est), '-')
set(gca, 'Xscale', 'log'); set(gca, 'Yscale', 'lin');
ylabel('Phase'); xlabel('Frequency [Hz]');
hold off;
@ -198,10 +198,10 @@ xlim([40, 400]);
% Model
wz = 2*pi*103;
xi_z = 0.01;
wp = 2*pi*237;
wp = 2*pi*238;
xi_p = 0.015;
Giff = -20*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);
Giff = 2*(s^2 + 2*xi_z*s*wz + wz^2)/(s^2 + 2*xi_p*s*wp + wp^2);
% Comparison model and identification
figure;
@ -232,7 +232,7 @@ hold on;
plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
for i = 1:length(gains)
cl_poles = pole(feedback(Giff, -gains(i)/(s + 2*pi*2)));
cl_poles = pole(feedback(Giff, gains(i)/(s + 2*pi*2)));
plot(real(cl_poles), imag(cl_poles), 'k.');
end
ylim([0, 1800]);
@ -240,9 +240,9 @@ xlim([-1600,200]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
% Optimal Controller
Kiff_opt = -110/(s + 2*pi*2);
Kiff_opt = 1100/(s + 2*pi*2);
%% New identification
id_ol = load('./mat/identification_chirp_40_400.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
@ -287,7 +287,7 @@ linkaxes([ax1,ax2], 'x');
xlim([40, 400]);
%% Estimation of the inertial sensor transfer functions
id = load('./mat/identification_noise_iff.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
id = load('./mat/identification_noise.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
ht = load('./mat/huddle_test.mat', 'd', 'acc_1', 'acc_2', 'geo_1', 'geo_2', 'f_meas', 'u', 't');
% Compare PSD

BIN
sensor_fusion_test_bench.slx
View File

Binary file not shown.

13
setup.m
View File

@ -4,15 +4,20 @@ Ts = 1e-4; % [s]
%% Pre-Filter
% Glpf = 1/(1 + s/2/pi/2e3);
Glpf = 1/(1 + s/2/pi/50); % Used to excite with constant velocity
Gz = c2d(Glpf, Ts, 'tustin');
G_pf = 1/(1 + s/2/pi/50); % Used to excite with constant velocity
G_pf = c2d(G_pf, Ts, 'tustin');
%% Force Sensor Filter (HPF)
Gf_hpf = s/(s + 2*pi*2);
Gf_hpf = c2d(Gf_hpf, Ts, 'tustin');
%% IFF Controller
Kiff = -1/(s + 2*pi*2);
Kiff = 1/(s + 2*pi*2);
Kiff = c2d(Kiff, Ts, 'tustin');
%% Excitation Signal
Tsim = 180; % Excitation time + Measurement time [s]
t = 0:Ts:Tsim;
u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);
% u_exc = timeseries(chirp(t, 0.1, Tsim, 1e3, 'logarithmic'), t);
u_exc = timeseries(chirp(t, 40, Tsim, 400, 'logarithmic'), t);