%Deflection based sensor placement optimization.

% Inputs:
% Deflection points and (x,y) points. 
% x1 and x2 are locations of the strain sensors along the needle where x1 <
% x2 < length of needle. 

% Outputs:
% 1.) Plot of actual deflection & est deflection (4th order) @ x1, x2.
% 2.) Plot of curvature & 2nd degree polynomial estimate curvature @ x1, x2.
% 4.) Plot of errors (y-yest).
% 5.) Sensitivity of the error between the actual tip deflection and 
% estimated deflection to the locations x1 and x2.

clear all; clc; close all;

% Model E (opposing deflection points)
% Extra points to xd.
%         x         y
D1 = [ 9.77304   0.68184;
      18.1824	 1.59096;
      25.45536   2.50008;
      33.86472   3.4092;
      42.50136	 3.86376];

% Extra points to xd2. 
%        x          y
D2 = [ 52.50168	 4.09104;
       62.27472	 3.193284;
       70.68408	 1.81824;
       79.09344	 0.22728;
       86.59368	 -1.386408;
       95.00304	 -3.4092;
      103.63968	 -5.90928;
      111.3672	 -9.0912;
      120.4584	 -12.04584;
      128.86776	 -15.00048;
      136.14072	 -17.50056;
      144.77736	 -20.68248;
      152.05032	 -23.63712];

syms xd yd xd2 yd2 L

% Make column of D1 data using y = a*x^3 + b*x^2:
% ex. x values = [(D1(:,1))^3 (D1(:,1))^2 0 0 0 0]
A1 = [];
for i = 1:length(D1);
    A1 = [A1; D1(i,1)^3 D1(i,1)^2  0 0 0 0];
end
Y1 = [];
for i = 1:length(D1);
    Y1 = [Y1; D1(i,2)];
end

% Make column of D2 data using y = d*x^3 + e*x^2 + fx + g:
A2 = [];
for i = 1:length(D2);
    A2 = [A2; 0 0 D2(i,1)^3 D2(i,1)^2 D2(i,1) 1];
end
Y2 = [];
for i = 1:length(D2);
    Y2 = [Y2; D2(i,2)];
end

% Set up matrix to solve for unknown coefficients. BC's of slopes &
% curvatures are determined from beam theory. 

Y = [yd; Y1; 0; 0; yd; yd2; Y2; 0];
A = [xd^3 xd^2 0 0 0 0;...
    A1;...
    3*xd^2 2*xd -3*xd^2 -2*xd 1 0;...
    6*xd 2 -6*xd -2 0 0;...
    0 0 xd^3 xd^2 xd 1;...
    0 0 xd2^3 xd2^2 xd2 1;...
    A2;...
    0 0 6*L 2 0 0];

xd = 52.5; yd = 4.09; xd2 = 142.27; yd2 = -18.75; L=150;
coeff = eval(A)\eval(Y);

syms x1 x2 x;
% x1: location of the first sensor
% x2: location of the second sensor
% x : variable along the needle length

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% True Model

%%% y = deflection
y1_eqn = coeff(1)*x^3 + coeff(2)*x^2;
y2_eqn = coeff(3)*x^3 + coeff(4)*x^2 + coeff(5)*x + coeff(6);

%%% s = slope
s1 = diff(y1_eqn);
s2 = diff(y2_eqn);

%%% rho_inv = curvature (6*a*x+2*b)
rho_inv1 = diff(s1);
rho_inv2 = diff(s2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Estimated Model from sensor signal

x = x1; rx1 = eval(rho_inv1); %These will be the known strain values
x = x2; rx2 = eval(rho_inv2);

X = [x1^2 x1 1; x2^2 x2 1; L^2 L 1];
A_hat = simplify( inv(X)*[rx1; rx2; 0] ); % Matrix of coefficients

syms x;
r_est = simplify( A_hat(1)*x^2 + A_hat(2)*x +A_hat(3) ); % Estimated curvature
s_est = simplify( int(r_est) );
y_est = simplify( int(s_est) ); % A 4th order approximation

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Error Calculation

x=L;
e=eval(y2_eqn)-eval(y_est);
err=eval(e);
dx1=diff(err,x1); % Sensitivity of err with respect to x1
dx2=diff(err,x2); % Sensitivity of err with respect to x2

d1=75; d2=150; % Assumes x1 will be placed in the first half of the needle. 
for x1=1:d1
    for x2=d1+1:d2
        Err(x2-d1, x1)=eval(err);
        DX1(x2-d1, x1)=eval(dx1);
        DX2(x2-d1, x1)=eval(dx2);
    end
end

% Plot of deflection errors. 
figure;
x1=1:d1; x2=d1+1:d2;
surfc(x1, x2, (Err), 'EdgeColor','none'); colorbar;
colormap hsv; shading interp;
xlabel('x1'); ylabel('x2'); axis([0 75 75 150 -15000 5000]);
zlabel('tip deflection error');
figure;
contourf(x1,x2, (Err), 'EdgeColor','none'); colorbar;
xlabel('x1');ylabel('x2');zlabel('tip deflection error');
colormap jet; grid on;

% Plot of sensitivity w/respect to x1. 
figure;
surfc(x1, x2, DX1, 'EdgeColor','none'); colorbar;
colormap hsv; shading interp;
xlabel('x1');ylabel('x2');zlabel('d error / d x1');
axis([0 75 75 150 -300000 100000]);
figure;
contourf(x1, x2, DX1); xlabel('x1');ylabel('x2'); 
colorbar; grid on;

% Plot of sensitivity w/respect to x2.
figure;
surfc(x1, x2, DX2, 'EdgeColor','none'); colorbar;
colormap hsv; shading interp;
xlabel('x1');ylabel('x2');zlabel('d error / d x2');
axis([0 75 75 150 -80000 20000]);
figure;
contourf(x1, x2, DX2); xlabel('x1');ylabel('x2'); 
colorbar; grid on;

% Plot deflections for optimal position x1 & x2.
xd = 52.5; yd = 4.09; xd2 = 142.27; yd2 = -18.75; L=150;
x1=30; x2=130; L = 150;
figure; hold on; grid on;
for x=0:.05:53
plot(x, eval(y1_eqn)),'b'; plot(x, eval(y_est), 'r');
end
for x = 53:0.5:150
plot(x, eval(y2_eqn)),'b'; plot(x, eval(y_est), 'r');
end
xlabel('x'); ylabel('Deflection');
legend('True Model','Estimated Model');

% Plot curvature for optimal position x1 & x2.
xd = 52.5; yd = 4.09; xd2 = 142.27; yd2 = -18.75; L=150;
x1=30; x2=130; L = 150;
figure; hold on; grid on;
for x=0:.05:53
plot(x, eval(rho_inv1)),'b'; plot(x, eval(r_est), 'r');
end
for x = 53:0.5:150
plot(x, eval(rho_inv2)),'b'; plot(x, eval(r_est), 'r');
end
%axis([0 150 -0.5 0.5]);
xlabel('x'); ylabel('Curvature');
legend('True Model','Estimated Model');