Commit aa1df1e6 authored by mmroma's avatar mmroma
Browse files

Working on haptic force estimation. Added Kalman Filter Method and it is working

parent b0927f40
function [F_User_est] = direcHapticForceCalc(data)
%direcHapticForceCalc
%% Directly Calculate Haptic Force Input
F_User_est = zeros(3,data.num_points);
for i=1:data.num_points
F_User_est(:,i) = data.m .* data.a_P(:,i) - data.F_Gravity - data.F_Drag_P(:,i) - data.F_Tethers(:,i);
end
end
function [params, simData] = hapticForceEstimation(params,simData)
%hapticForceEstimation
%% 1) Obtain Payload Acceleration from Actual Net Force, Rotation, and Mass
F_net_P_P = simData.F_nets(params.num_vehicles*3+1:end,:);
m_P = params.payload.m;
X_P = simData.X(params.num_vehicles*12+1:end,:);
num_points = size(X_P,2);
a_P_G = zeros(3,num_points);
for i=1:num_points
R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
a_P_G(:,i) = R_P_G * F_net_P_P(:,i) ./ m_P;
end
%% 2) Gather Remaing Known Forces
% Tether Forces
F_Tethers_G = zeros(3,num_points);
for i=1:num_points
R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
F_Tethers_G(:,i) = R_P_G * simData.F_tethers(params.num_vehicles*3+1:end,i);
end
% Gravity
F_Gravity_G = m_P * [0; 0; params.g];
% Drag
F_Drag_P_G = zeros(3,num_points);
for i=1:num_points
R_P_G = rpy_to_R(X_P(7), X_P(8), X_P(9));
F_Drag_P_G(:,i) = R_P_G * simData.F_drags(params.num_vehicles*3+1:end,i);
end
%% 3) Calculate Haptic Force Input
F_User = zeros(3,num_points);
for i=1:num_points
F_User(:,i) = m_P .* a_P_G(:,i) - F_Gravity_G - F_Drag_P_G(:,i) - F_Tethers_G(:,i);
end
%% 4) Plot Results
figure(1);
dims = {'X','Y','Z'};
for i=1:3
subplot(3,1,i);
plot(simData.T,F_User(i,:));
hold on;
plot(simData.T,params.user_force(i,:));
hold off;
legend('Estimate','Actual');
xlabel('Time (s)');
ylabel('Force (N)');
title([dims{i} ' Direction - Haptic Force Input']);
end
% figure(2);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T,a_P_G(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Acceleration']);
% end
%
% figure(3);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T, F_Tethers_G(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Tethers']);
% end
% figure(4);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T, F_net_P_P(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Net Force in Payload Frame']);
% end
%
% F_Grav_P_P = zeros(3,num_points);
% for i=1:num_points
% R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
% F_Grav_P_P(:,i) = R_P_G' * m_P * [0; 0; params.g];
% end
% figure(5);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T, F_Grav_P_P(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Gravity Force in Payload Frame']);
% end
%
% subplot(3,1,2);
% plot(simData.T,F_User(1,:));
% hold on;
% plot(simData.T,params.user_force(1,:));
% hold off;
% legend('Estimate','Actual');
% xlabel('Time (s)');
% ylabel('Force (N)');
% title('X Direction - Haptic Force Input');
%
% subplot(3,1,3);
% plot(simData.T,F_User(1,:));
% hold on;
% plot(simData.T,params.user_force(1,:));
% hold off;
% legend('Estimate','Actual');
% xlabel('Time (s)');
% ylabel('Force (N)');
% title('X Direction - Haptic Force Input');
%% Pre Process Data
estimationData = preProcessHapticForceEstimation(params,simData);
%% Level 0) Direct Haptic Force Calculation w/ No Filter
F_User_est0 = direcHapticForceCalc(estimationData);
plotHapticForceEstimate(estimationData, F_User_est0);
%% Level 1) Kalman Filter Haptic Force Calculation
[F_User_est1, F_User_est1_Sigma] = kalmanFilterHapticForceCalc(estimationData);
plotHapticForceEstimate(estimationData, F_User_est1);
plotHapticForceEstimateUncertainty(estimationData, F_User_est1_Sigma);
end
function [F_User_est, F_User_est_Sigma] = kalmanFilterHapticForceCalc(data)
%kalmanFilterHapticForceCalc
% linear payload dynamics kalman filter v3.0 in overleaf
%% Define System Matrices
A = [eye(3), zeros(3,3), zeros(3,3);
data.m.*eye(3), zeros(3,3), -eye(3);
zeros(3,3), zeros(3,3), eye(3)];
B = [zeros(3,3);
-eye(3);
zeros(3,3)];
C = [eye(3), zeros(3,3), zeros(3,3);
zeros(3,3), zeros(3,3), eye(3)];
%% Define Uncertainty Matrices
n = size(A,1);
m = size(C,1);
% Dynamics Uncertainty
Q = [eye(3), zeros(3,3), zeros(3,3);
zeros(3,3), eye(3), zeros(3,3);
zeros(3,3), zeros(3,3), eye(3)];
% Measurement Uncertainty
R = [eye(3), zeros(3,3);
zeros(3,3), eye(3)];
%% Grab "Measurements"
z = [data.a_P;
data.F_Tethers];
%% Zero out user force estimates
F_User_est = zeros(3,data.num_points);
F_User_est_Sigma = cell(data.num_points,1);
%% Run the Kalman Filter
Sigma_t = Q;
Mu_t = [zeros(3,1);
zeros(3,1);
-data.F_Gravity];
u_t = data.F_Gravity;
for i=1:data.num_points
if i==499
disp("Help");
end
% Predict
Mu_t1_hat = A * Mu_t + B * u_t;
Sigma_t1_hat = A * Sigma_t * A' + Q;
z_t_hat = C * Mu_t1_hat;
% Update (Use Data)
K = Sigma_t1_hat * C' / (C*Sigma_t1_hat*C' + R);
Mu_t1 = Mu_t1_hat + K * (z(:,i) - z_t_hat);
Sigma_t1 = (eye(n) - K*C) * Sigma_t1_hat;
% Grab the Data
F_User_est(:,i) = Mu_t1(4:6,1);
F_User_est_Sigma{i} = Sigma_t1(4:6,4:6);
% t+1 -> t for the next iteration
Mu_t = Mu_t1;
Sigma_t = Sigma_t1;
end
end
function [] = plotHapticForceEstimate(data, F_User_est)
%plotHapticForceEstimate
%% 4) Plot Results
figure();
dims = {'X','Y','Z'};
for i=1:3
subplot(3,1,i);
plot(data.T,F_User_est(i,:));
hold on;
plot(data.T,data.F_User(i,:));
hold off;
legend('Estimate','Actual');
xlabel('Time (s)');
ylabel('Force (N)');
title([dims{i} ' Direction - Haptic Force Input']);
end
% figure(2);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T,a_P_G(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Acceleration']);
% end
%
% figure(3);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T, F_Tethers_G(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Tethers']);
% end
% figure(4);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T, F_net_P_P(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Net Force in Payload Frame']);
% end
%
% F_Grav_P_P = zeros(3,num_points);
% for i=1:num_points
% R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
% F_Grav_P_P(:,i) = R_P_G' * m_P * [0; 0; params.g];
% end
% figure(5);
% dims = {'X','Y','Z'};
% for i=1:3
% subplot(3,1,i);
% plot(simData.T, F_Grav_P_P(i,:));
% xlabel('Time (s)');
% ylabel('Force (N)');
% title([dims{i} ' Direction - Gravity Force in Payload Frame']);
% end
%
% subplot(3,1,2);
% plot(simData.T,F_User(1,:));
% hold on;
% plot(simData.T,params.user_force(1,:));
% hold off;
% legend('Estimate','Actual');
% xlabel('Time (s)');
% ylabel('Force (N)');
% title('X Direction - Haptic Force Input');
%
% subplot(3,1,3);
% plot(simData.T,F_User(1,:));
% hold on;
% plot(simData.T,params.user_force(1,:));
% hold off;
% legend('Estimate','Actual');
% xlabel('Time (s)');
% ylabel('Force (N)');
% title('X Direction - Haptic Force Input');
end
function [] = plotHapticForceEstimateUncertainty(data, F_User_est_Sigma)
%plotHapticForceEstimateUncertainty
%% Grab Each Dimension
Sigmas = zeros(3,data.num_points);
for i=1:data.num_points
Sigmas(1,i) = F_User_est_Sigma{i}(1,1);
Sigmas(2,i) = F_User_est_Sigma{i}(2,2);
Sigmas(3,i) = F_User_est_Sigma{i}(3,3);
end
figure();
dims = {'X','Y','Z'};
for i=1:3
subplot(3,1,i);
plot(data.T,Sigmas(i,:));
xlabel('Time (s)');
ylabel('Sigma');
title([dims{i} ' Direction - Haptic Force Input Uncertainty']);
end
end
function [estimationData] = preProcessHapticForceEstimation(params,simData)
% %% 0) Grab sampling time
Ts = params.multirotors{1}.ctlDt;
%% 1) Obtain Payload Acceleration from Actual Net Force, Rotation, and Mass
F_net_P_P = simData.F_nets(params.num_vehicles*3+1:end,:);
m_P = params.payload.m;
X_P = simData.X(params.num_vehicles*12+1:end,:);
num_points = size(X_P,2);
a_P_G = zeros(3,num_points);
for i=1:num_points
R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
a_P_G(:,i) = R_P_G * F_net_P_P(:,i) ./ m_P;
end
%% 2) Gather Remaing Known Forces
% Tether Forces
F_Tethers_G = zeros(3,num_points);
for i=1:num_points
R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
F_Tethers_G(:,i) = R_P_G * simData.F_tethers(params.num_vehicles*3+1:end,i);
end
% Gravity
F_Gravity_G = m_P * [0; 0; params.g];
% Drag
F_Drag_P_G = zeros(3,num_points);
for i=1:num_points
R_P_G = rpy_to_R(X_P(7), X_P(8), X_P(9));
F_Drag_P_G(:,i) = R_P_G * simData.F_drags(params.num_vehicles*3+1:end,i);
end
%% 3) Send to variable
estimationData.Ts = Ts;
estimationData.m = m_P;
estimationData.a_P = a_P_G;
estimationData.F_Gravity = F_Gravity_G;
estimationData.F_Drag_P = F_Drag_P_G;
estimationData.F_Tethers = F_Tethers_G;
estimationData.F_User = params.user_force;
estimationData.T = simData.T;
estimationData.num_points = num_points;
end
%% Test hapticForceEstimation
close all;
%% 1) Load the simData and params
load('run1.mat');
load('run2.mat');
%% 2) Call the function
[params, simData] = hapticForceEstimation(params,simData);
\ No newline at end of file
......@@ -23,7 +23,7 @@ function [params] = setUpParameters(traj_num)
%% Noise and Disturbance Forces
quad_dist_STD = 0; %0.1; % 0-mean White Gaussian Quad Disturbance
load_dist_STD = 0; %0.1; % 0-mean White Gaussian Load Disturbance
load_dist_STD = 0.1; %0.1; % 0-mean White Gaussian Load Disturbance
%% Sensor Noise on Vehicle States (0-mean White Gaussian Noise)
std = 0;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment