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

Estimation/direcHapticForceCalc.m

+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

 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

+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

+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

+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

+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

 %% 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

Parameters/setUpParameters.m

@@ -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;