47 lines
1.2 KiB
Matlab
47 lines
1.2 KiB
Matlab
figure;
|
|
plot(d_meas.Time, d.Data-d_meas.Data)
|
|
|
|
%%
|
|
figure;
|
|
plot(error.Time, error.Data)
|
|
legend({'x', 'y', 'z', 'theta_x', 'theta_y', 'theta_z'})
|
|
|
|
%%
|
|
J = jacobian.Data(:, :, 1);
|
|
|
|
% Norm of the jacobian with time
|
|
J_change = (jacobian.Data - J)./J;
|
|
|
|
figure;
|
|
hold on;
|
|
plot(jacobian.Time, squeeze(J_change(1, 1, :)));
|
|
plot(jacobian.Time, squeeze(J_change(2, 2, :)));
|
|
plot(jacobian.Time, squeeze(J_change(3, 3, :)));
|
|
plot(jacobian.Time, squeeze(J_change(4, 4, :)));
|
|
plot(jacobian.Time, squeeze(J_change(5, 5, :)));
|
|
plot(jacobian.Time, squeeze(J_change(6, 6, :)));
|
|
legend({'Jxx', 'Jyy', 'Jzz', 'Jmx', 'Jmy', 'Jmz'})
|
|
hold off;
|
|
|
|
%% K change
|
|
K_init = J'*J;
|
|
K = zeros(size(jacobian.Data));
|
|
|
|
for i=1:length(jacobian.Time)
|
|
K(:, :, i) = jacobian.Data(:, :, i)'*jacobian.Data(:, :, i);
|
|
end
|
|
|
|
K_change = (permute(K, [2, 1, 3]) - K_init)./K_init;
|
|
|
|
figure;
|
|
hold on;
|
|
plot(jacobian.Time, squeeze(K_change(1, 1, :)));
|
|
plot(jacobian.Time, squeeze(K_change(2, 2, :)));
|
|
plot(jacobian.Time, squeeze(K_change(3, 3, :)));
|
|
plot(jacobian.Time, squeeze(K_change(4, 4, :)));
|
|
plot(jacobian.Time, squeeze(K_change(5, 5, :)));
|
|
plot(jacobian.Time, squeeze(K_change(6, 6, :)));
|
|
legend({'Kxx', 'Kyy', 'Kzz', 'Kmx', 'Kmy', 'Kmz'})
|
|
hold off;
|
|
|