diff --git a/sw/in_progress/inertial/scilab/calibrate_mag.sce b/sw/in_progress/inertial/scilab/calibrate_mag.sce new file mode 100644 index 0000000000..0c62314b38 --- /dev/null +++ b/sw/in_progress/inertial/scilab/calibrate_mag.sce @@ -0,0 +1,61 @@ +clear(); +exec("calibration_utils.sci"); + +[raw_mag, raw_accel] = read_log("log_calib_accel_mag_2.dat"); + +[fraw_mag] = filter_noise(raw_mag,15,300); + + +n0 = [ 1950; 2000; 2150]; +g0 = [ 2; 2; 2]; + +[scaled_mag] = scale_sensor(fraw_mag, g0, n0); +[scaled_module] = compute_mod(scaled_mag); + + +function err = cost_fun(p, z) + err = (z(1) - p(1))^2 * p(4)^2 + ... + (z(2) - p(2))^2 * p(5)^2 + ... + (z(3) - p(3))^2 * p(6)^2 - (2^11)^2; +endfunction + +[p, err] = datafit(cost_fun, fraw_mag, [n0; g0]); + +[scaled_mag2] = scale_sensor(fraw_mag, p(4:6), p(1:3)); +[scaled_module2] = compute_mod(scaled_mag2); + + +gain_foo = [ 2^11/p(4) + 2^11/p(5) + 2^11/p(6) ] + +clf(); + +subplot(4,1,1); +[nl, nc] = size(raw_mag); +plot2d(1:nc, raw_mag(1,:), 1); +plot2d(1:nc, raw_mag(2,:), 2); +plot2d(1:nc, raw_mag(3,:), 3); + +subplot(4,1,2); +[nl, nc] = size(fraw_mag); +plot2d(1:nc, fraw_mag(1,:), 1); +plot2d(1:nc, fraw_mag(2,:), 2); +plot2d(1:nc, fraw_mag(3,:), 3); + +subplot(4,1,3); +plot2d(1:nc, scaled_mag(1,:), 1); +plot2d(1:nc, scaled_mag(2,:), 2); +plot2d(1:nc, scaled_mag(3,:), 3); + +plot2d(1:nc, scaled_mag2(1,:), 4); +plot2d(1:nc, scaled_mag2(2,:), 5); +plot2d(1:nc, scaled_mag2(3,:), 6); + + + +subplot(4,1,4); +plot2d(1:nc, scaled_module, 1); + +subplot(4,1,4); +plot2d(1:nc, scaled_module2, 2); diff --git a/sw/in_progress/inertial/scilab/calibration_utils.sci b/sw/in_progress/inertial/scilab/calibration_utils.sci new file mode 100644 index 0000000000..d938743a29 --- /dev/null +++ b/sw/in_progress/inertial/scilab/calibration_utils.sci @@ -0,0 +1,65 @@ +function [raw_mag, raw_accel] = read_log(filename) + + raw_mag = []; + raw_accel = []; + + u=mopen(filename,'r'); + + while meof(u) == 0, + line = mgetl(u, 1); + if strindex(line, '#') ~= 1 & length(line) ~= 0, + [nb_scan, _time, _mz, _my, _mx] = msscanf(1, line, '%f 149 IMU_MAG_RAW %f %f %f'); + if nb_scan == 4, + raw_mag = [raw_mag [_mx _my _mz]']; + else + [nb_scan, _time, _ax, _ay, _az] = msscanf(1, line, '%f 149 IMU_ACCEL_RAW %f %f %f'); + if nb_scan == 4, + raw_accel = [raw_accel [_ax _ay _az]']; + end + end + end + end + + mclose(u); + +endfunction + + + +function [fraw_sensor] = filter_noise(raw_sensor, window_size, max_var) + + [nl, nc] = size(raw_sensor); + fraw_sensor = []; + + for i=window_size+1:nc-window_size-1 + + v = variance(raw_sensor(:,i-window_size:i+window_size),'c'); + + if norm(v) < max_var + fraw_sensor = [fraw_sensor raw_sensor(:,i)]; + end + end + +endfunction + + +function [scaled_sensor] = scale_sensor(raw_sensor, g, n) + + [nl, nc] = size(raw_sensor); + scale_sensor = zeros(nl, nc); + + for i=1:nc + scaled_sensor(:,i) = g .* (raw_sensor(:,i) - n); + end + +endfunction + +function [_mod] = compute_mod(sensor) + + [nl, nc] = size(sensor); + _mod = zeros(1,nc); + for i=1:nc + _mod(i) = norm(sensor(:,i)); + end + +endfunction