Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
mmroma
coop_payload_sim
Commits
aa1df1e6
Commit
aa1df1e6
authored
Aug 03, 2020
by
mmroma
Browse files
Working on haptic force estimation. Added Kalman Filter Method and it is working
parent
b0927f40
Changes
8
Hide whitespace changes
Inline
Side-by-side
Estimation/direcHapticForceCalc.m
0 → 100644
View file @
aa1df1e6
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
Estimation/hapticForceEstimation.m
View file @
aa1df1e6
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
Estimation/kalmanFilterHapticForceCalc.m
0 → 100644
View file @
aa1df1e6
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
Estimation/plotHapticForceEstimate.m
0 → 100644
View file @
aa1df1e6
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
Estimation/plotHapticForceEstimateUncertainty.m
0 → 100644
View file @
aa1df1e6
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
Estimation/preProcessHapticForceEstimation.m
0 → 100644
View file @
aa1df1e6
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
Estimation/testHapticForceEstimation.m
View file @
aa1df1e6
%% Test hapticForceEstimation
close
all
;
%% 1) Load the simData and params
load
(
'run
1
.mat'
);
load
(
'run
2
.mat'
);
%% 2) Call the function
[
params
,
simData
]
=
hapticForceEstimation
(
params
,
simData
);
\ No newline at end of file
Parameters/setUpParameters.m
View file @
aa1df1e6
...
...
@@ -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
;
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment