preProcessHapticForceEstimation.m 1.83 KB
Newer Older
1
2
3
4
5
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
6
7
%     F_net_P_P = simData.F_nets(params.num_vehicles*3+1:end,:);
    F_net_P_P = simData.F_net_payload';
8
    m_P = params.payload.m;
9
10
%     X_P = simData.X(params.num_vehicles*12+1:end,:);
    X_P = simData.payload_X';
11
12
13
14
15
16
17
18
19
20
21
22
23
    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);
24
25
26
    
    
    simData.F_tethers_P
27
28
    for i=1:num_points
        R_P_G = rpy_to_R(X_P(7,i), X_P(8,i), X_P(9,i));
29
        F_Tethers_G(:,i) = R_P_G * simData.F_tethers_P(i,:)';
30
31
32
33
34
35
36
37
38
39
    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));
40
       F_Drag_P_G(:,i) = R_P_G * simData.F_drag_payload(i,:)';
41
42
    end
    
43
44
45
46
47
48
49
50
51
52
    %% 3) Resample at controller Rate (interpolate)
    t = simData.tout(1) : Ts : simData.tout(end);
    num_points = length(t);
   
    a_P_G = interp1(simData.tout,a_P_G',t);
    F_Drag_P_G = interp1(simData.tout,F_Drag_P_G',t);
    F_Tethers_G = interp1(simData.tout,F_Tethers_G',t);
  
    
    %% 4) Send to variable
53
54
    estimationData.Ts = Ts;
    estimationData.m = m_P;
55
    estimationData.a_P = a_P_G';
56
    estimationData.F_Gravity = F_Gravity_G;
57
58
    estimationData.F_Drag_P = F_Drag_P_G';
    estimationData.F_Tethers = F_Tethers_G';
59
    estimationData.F_User = params.user_force;
60
    estimationData.T = t;
61
62
63
    estimationData.num_points = num_points;
end