I am trying to plot a state-space diagram, as well as a time-history diagram, of a dynamical system. There's a catch, though. The state-space is divided into two halves by a plane located at x1 = 0. The state-space axes are x1, x2, x3. The x1 = 0 plane is parallel to the x2/x3 plane. The state-space above the x1 = 0 plane is described by the ODEs in eqx3, whereas the state-space below the x1 = 0 plane is described by the ODEs in eqx4.
So, there is a discontinuity on the plane x1 = 0. I have a vague understanding that an event function (function [value,isterminal,direction] = myEventsFcn(t,y)) should be used, but I do not know what values to give to "value", "isterminal", and "direction".
In my code below, I have an initial condition for eqx3, and another initial condition for eqx4. The initial condition for eqx3 is in the upper half of the state-space (x1 > 0). The orbit then hits the x1 = 0 plane and there is a discontinuity, and the eqx4 trajectory begins on this plane, but from a different point from where eqx3 ended.
Can this be done? How do I put it into a code? Do I stop the integration when the orbit reaches the plane x1 = 0?
eta = 0.05;
omega = 25;
tspan = [0,50];
initcond = [2, 3, 4]
[t,x] = ode45(#(t,x) eqx3(t,x,eta, omega), tspan, initcond);
initcond1 = [0, 1, 1]
[t,y] = ode45(#(t,y) eqx4(t,y,eta, omega), tspan, initcond1);
plot3(x(:,1), x(:,2), x(:,3),y(:,1), y(:,2), y(:,3))
xlabel('x1')
ylabel('x2')
zlabel('x3')
%subplot(222)
%plot(t, x(:,1), t,x(:,2),t,x(:,3),'--');
%xlabel('t')
function xdot = eqx3(t,x,eta,omega)
xdot = zeros(3,1);
xdot(1) = -(2*eta*omega + 1)*x(1) + x(2) - 1;
xdot(2) = -(2*eta*omega + (omega^2))*x(1) + x(3) + 2;
xdot(3) = -(omega^2)*x(1) + x(2) - 1;
end
function ydot = eqx4(t,y,eta,omega)
ydot = zeros(3,1);
ydot(1) = -(2*eta*omega + 1)*y(1) + y(2) + 1;
ydot(2) = -(2*eta*omega + (omega^2))*y(1) + y(3) - 2;
ydot(3) = -(omega^2)*y(1) + y(2) + 1;
end
function [value,isterminal,direction] = myEventsFcn(t,y)
value = 0
isterminal = 1
direction = 1
end
Without events, using a close-by smooth system
First, as the difference between the systems is the addition or subtraction of a constant vector it is easy to find an approximate smooth version of the system using some sigmoid function like tanh.
eta = 0.05;
omega = 25;
t0=0; tf=4;
initcond = [2, 3, 4];
opt = odeset('AbsTol',1e-11,'RelTol',1e-13);
opt = odeset(opt,'MaxStep',0.005); % in Matlab: opt = odeset('Refine',10);
[t,x] = ode45(#(t,x) eqx34(t,x,eta, omega), [t0, tf], initcond, opt);
clf;
subplot(121);
plot3(x(:,1), x(:,2), x(:,3));
xlabel('x1');
ylabel('x2');
zlabel('x3');
subplot(122);
plot(t, 10.*x(:,1), t,x(:,2),':',t,x(:,3),'--');
xlabel('t');
function xdot = eqx34(t,x,eta,omega)
S = tanh(1e6*x(1));
xdot = zeros(3,1);
xdot(1) = -(2*eta*omega + 1)*x(1) + x(2) - S;
xdot(2) = -(2*eta*omega + (omega^2))*x(1) + x(3) + 2*S;
xdot(3) = -(omega^2)*x(1) + x(2) - S;
end
resulting in the plots
As is visible, after t=1.2 the solution is essentially constant with x(1)=0 and the other coordinates close to zero.
With events
If you want to use the event mechanism, make ODE and event function dependent on a sign parameter S denoting the phase and the direction of the zero crossing.
eta = 0.05;
omega = 25;
t0=0; tf=4;
initcond = [2, 3, 4];
opt = odeset('AbsTol',1e-10,'RelTol',1e-12,'InitialStep',1e-6);
opt = odeset(opt,'MaxStep',0.005); % in Matlab: opt = odeset('Refine',10);
T = [t0]; TE = [];
X = [initcond]; XE = [];
S = 1; % sign of x(1)
while t0<tf
opt = odeset(opt,'Events', #(t,x)myEventsFcn(t,x,S));
[t,x,te,xe,ie] = ode45(#(t,x) eqx34(t,x,eta, omega, S), [t0, tf], initcond, opt);
T = [ T; t(2:end) ]; TE = [ TE; te ];
X = [ X; x(2:end,:)]; XE = [ XE; xe ];
t0 = t(end);
initcond = x(end,:);
S = -S % alternatively = 1-2*(initcond(1)<0);
end
disp(TE); disp(XE);
subplot(121);
hold on;
plot3(X(:,1), X(:,2), X(:,3),'b-');
plot3(XE(:,1), XE(:,2), XE(:,3),'or');
hold off;
xlabel('x1');
ylabel('x2');
zlabel('x3');
subplot(122);
plot(T, 10.*X(:,1), T,X(:,2),':',T,X(:,3),'--');
xlabel('t');
function xdot = eqx34(t,x,eta,omega,S)
xdot = zeros(3,1);
xdot(1) = -(2*eta*omega + 1)*x(1) + x(2) - S;
xdot(2) = -(2*eta*omega + (omega^2))*x(1) + x(3) + 2*S;
xdot(3) = -(omega^2)*x(1) + x(2) - S;
end
function [value,isterminal,direction] = myEventsFcn(t,x,S)
value = x(1)+2e-4*S;
isterminal = 1;
direction = -S;
end
The solution enters a sliding mode for 1.36 < t < 1.43 where (theoretically) x(1)=0 and the vector field points to the other phase from both sides. The theoretical solution is to take a linear combination that sets the first component to zero, so that the resulting direction is tangential to the separating surface. In the first variant above the sigmoid achieves something like this automatically. Using events one could add additional event functions that test the vector field for these conditions, and when they cease to persist.
A quick solution is to thicken the boundary surface, that is, test for x(1)+epsilon*S==0 so that the solution has to cross the boundary surface before triggering the event. In sliding mode, it will be immediately pushed back, giving a ping-pong or zigzag motion. epsilon has to be small to not perturb the solution too much, however not too small to avoid the triggering of too many events. With epsilon=2e-4 octave gives the following solution in a close-up to the sliding interval
The octave solver, and in some way also Matlab, will not trigger a terminal event if it happens in the first integration step. For that reason the InitialStep option was set to a rather small value, it should be about 0.01*epsilon. The full solution looks now similar to the one obtained in the first variant
Related
Consider the following problem:
I am now in the third part of this question. I wrote the vectorial loop equations (q=teta2, x=teta3 and y=teta4):
fval(1,1) = r2*cos(q)+r3*cos(x)-r4*cos(y)-r1;
fval(2,1) = r2*sin(q)+r3*sin(x)-r4*sin(y);
I have these 2 functions, and all variables except x and y are given. I found the roots with help of this video.
Now I need to plot graphs of q versus x and q versus y when q is at [0,2pi] with delta q of 2.5 degree. What should I do to plot the graphs?
Below is my attempt so far:
function [fval,jac] = lorenzSystem(X)
%Define variables
x = X(1);
y = X(2);
q = pi/2;
r2 = 15
r3 = 50
r4 = 45
r1 = 40
%Define f(x)
fval(1,1)=r2*cos(q)+r3*cos(x)-r4*cos(y)-r1;
fval(2,1)=r2*sin(q)+r3*sin(x)-r4*sin(y);
%Define Jacobian
jac = [-r3*sin(X(1)), r4*sin(X(2));
r3*cos(X(1)), -r4*cos(X(2))];
%% Multivariate NR
%Initial conditions:
X0 = [0.5;1];
maxIter = 50;
tolX = 1e-6;
X = X0;
Xold = X0;
for i = 1:maxIter
[f,j] = lorenzSystem(X);
X = X - inv(j)*f;
err(:,i) = abs(X-Xold);
Xold = X;
if (err(:,i)<tolX)
break;
end
end
Please take a look at my solution below, and study how it differs from your own.
function [th2,th3,th4] = q65270276()
[th2,th3,th4] = lorenzSystem();
hF = figure(); hAx = axes(hF);
plot(hAx, deg2rad(th2), deg2rad(th3), deg2rad(th2), deg2rad(th4));
xlabel(hAx, '\theta_2')
xticks(hAx, 0:pi/3:2*pi);
xticklabels(hAx, {'$0$','$\frac{\pi}{3}$','$\frac{2\pi}{3}$','$\pi$','$\frac{4\pi}{3}$','$\frac{5\pi}{3}$','$2\pi$'});
hAx.TickLabelInterpreter = 'latex';
yticks(hAx, 0:pi/6:pi);
yticklabels(hAx, {'$0$','$\frac{\pi}{6}$','$\frac{\pi}{3}$','$\frac{\pi}{2}$','$\frac{2\pi}{3}$','$\frac{5\pi}{6}$','$\pi$'});
set(hAx, 'XLim', [0 2*pi], 'YLim', [0 pi], 'FontSize', 16);
grid(hAx, 'on');
legend(hAx, '\theta_3', '\theta_4')
end
function [th2,th3,th4] = lorenzSystem()
th2 = (0:2.5:360).';
[th3,th4] = deal(zeros(size(th2)));
% Define geometry:
r1 = 40;
r2 = 15;
r3 = 50;
r4 = 45;
% Define the residual:
res = #(q,X)[r2*cosd(q)+r3*cosd(X(1))-r4*cosd(X(2))-r1; ... Δx=0
r2*sind(q)+r3*sind(X(1))-r4*sind(X(2))]; % Δy=0
% Define the Jacobian:
J = #(X)[-r3*sind(X(1)), r4*sind(X(2));
r3*cosd(X(1)), -r4*cosd(X(2))];
X0 = [acosd((45^2-25^2-50^2)/(-2*25*50)); 180-acosd((50^2-25^2-45^2)/(-2*25*45))]; % Accurate guess
maxIter = 500;
tolX = 1e-6;
for idx = 1:numel(th2)
X = X0;
Xold = X0;
err = zeros(maxIter, 1); % Preallocation
for it = 1:maxIter
% Update the guess
f = res( th2(idx), Xold );
X = Xold - J(Xold) \ f;
% X = X - pinv(J(X)) * res( q(idx), X ); % May help when J(X) is close to singular
% Determine convergence
err(it) = (X-Xold).' * (X-Xold);
if err(it) < tolX
break
end
% Update history
Xold = X;
end
% Unpack and store θ₃, θ₄
th3(idx) = X(1);
th4(idx) = X(2);
% Update X0 for faster convergence of the next case:
X0 = X;
end
end
Several notes:
All computations are performed in degrees.
The specific plotting code I used is less interesting, what matters is that I defined all θ₂ in advance, then looped over them to find θ₃ and θ₄ (without recursion, as was done in your own implementation).
The initial guess (actually, analytical solution) for the very first case (θ₂=0) can be found by solving the problem manually (i.e. "on paper") using the law of cosines. The solver also works for other guesses, but you might need to increase maxIter. Also, for certain guesses (e.g. X(1)==X(2)), the Jacobian is ill-conditioned, in which case you can use pinv.
If my computation is correct, this is the result:
Introduction
I am using Matlab to simulate some dynamic systems through numerically solving systems of Second Order Ordinary Differential Equations using ODE45. I found a great tutorial from Mathworks (link for tutorial at end) on how to do this.
In the tutorial the system of equations is explicit in x and y as shown below:
x''=-D(y) * x' * sqrt(x'^2 + y'^2)
y''=-D(y) * y' * sqrt(x'^2 + y'^2) + g(y)
Both equations above have form y'' = f(x, x', y, y')
Question
However, I am coming across systems of equations where the variables can not be solved for explicitly as shown in the example. For example one of the systems has the following set of 3 second order ordinary differential equations:
y double prime equation
y'' - .5*L*(x''*sin(x) + x'^2*cos(x) + (k/m)*y - g = 0
x double prime equation
.33*L^2*x'' - .5*L*y''sin(x) - .33*L^2*C*cos(x) + .5*g*L*sin(x) = 0
A single prime is first derivative
A double prime is second derivative
L, g, m, k, and C are given parameters.
How can Matlab be used to numerically solve a set of second order ordinary differential equations where second order can not be explicitly solved for?
Thanks!
Your second system has the form
a11*x'' + a12*y'' = f1(x,y,x',y')
a21*x'' + a22*y'' = f2(x,y,x',y')
which you can solve as a linear system
[x'', y''] = A\f
or in this case explicitly using Cramer's rule
x'' = ( a22*f1 - a12*f2 ) / (a11*a22 - a12*a21)
y'' accordingly.
I would strongly recommend leaving the intermediate variables in the code to reduce chances for typing errors and avoid multiple computation of the same expressions.
Code could look like this (untested)
function dz = odefunc(t,z)
x=z(1); dx=z(2); y=z(3); dy=z(4);
A = [ [-.5*L*sin(x), 1] ; [.33*L^2, -0.5*L*sin(x)] ]
b = [ [dx^2*cos(x) + (k/m)*y-g]; [-.33*L^2*C*cos(x) + .5*g*L*sin(x)] ]
d2 = A\b
dz = [ dx, d2(1), dy, d2(2) ]
end
Yes your method is correct!
I post the following code below:
%Rotating Pendulum Sym Main
clc
clear all;
%Define parameters
global M K L g C;
M = 1;
K = 25.6;
L = 1;
C = 1;
g = 9.8;
% define initial values for theta, thetad, del, deld
e_0 = 1;
ed_0 = 0;
theta_0 = 0;
thetad_0 = .5;
initialValues = [e_0, ed_0, theta_0, thetad_0];
% Set a timespan
t_initial = 0;
t_final = 36;
dt = .01;
N = (t_final - t_initial)/dt;
timeSpan = linspace(t_final, t_initial, N);
% Run ode45 to get z (theta, thetad, del, deld)
[t, z] = ode45(#RotSpngHndl, timeSpan, initialValues);
%initialize variables
e = zeros(N,1);
ed = zeros(N,1);
theta = zeros(N,1);
thetad = zeros(N,1);
T = zeros(N,1);
V = zeros(N,1);
x = zeros(N,1);
y = zeros(N,1);
for i = 1:N
e(i) = z(i, 1);
ed(i) = z(i, 2);
theta(i) = z(i, 3);
thetad(i) = z(i, 4);
T(i) = .5*M*(ed(i)^2 + (1/3)*L^2*C*sin(theta(i)) + (1/3)*L^2*thetad(i)^2 - L*ed(i)*thetad(i)*sin(theta(i)));
V(i) = -M*g*(e(i) + .5*L*cos(theta(i)));
E(i) = T(i) + V(i);
end
figure(1)
plot(t, T,'r');
hold on;
plot(t, V,'b');
plot(t,E,'y');
title('Energy');
xlabel('time(sec)');
legend('Kinetic Energy', 'Potential Energy', 'Total Energy');
Here is function handle file for ode45:
function dz = RotSpngHndl(~, z)
% Define Global Parameters
global M K L g C
A = [1, -.5*L*sin(z(3));
-.5*L*sin(z(3)), (1/3)*L^2];
b = [.5*L*z(4)^2*cos(z(3)) - (K/M)*z(1) + g;
(1/3)*L^2*C*cos(z(3)) + .5*g*L*sin(z(3))];
X = A\b;
% return column vector [ed; edd; ed; edd]
dz = [z(2);
X(1);
z(4);
X(2)];
I'm trying to make a program to solve the following system with ode23:
2y’ + z’–y + 2z = 0
y’ + 3z’ –3y +z = 0
with initial values:
y(0) = 1
z(0) = 0
and analytic solution:
y= cos(x)
z= sin(x)
but when I change the variable 4:
function dy = eqdif2(t,y)
%2y’ + z’ –y + 2z = 0;
%y’ + 3z’ –3y +z = 0
% y(0) = 1, Z(0) = 0
% y=y(1), z=y(2), y'=y(3), z'=y(4)
dy = [-2*y(3)+y(1)-2*y(2);3*y(1)-y(2)-3*y(4)];
I have a problem with ode23, reporting only 2 solutions:
clc,clear;
yp = [1 0]; %initial values
options = odeset('RelTol', 1e-4);
[t,y]= ode23('eqdif2',[0 20],yp,options);
ya=cos(x);
za=sin(x);
figure;
plot(t,y(:,1),'-');
figure;
plot(t,ya,t,za);
Here we go...
The ODE are linear, we know it, there is an algebraic solution and so. For using ode23 you need an explicit set, on the form -- ps. note the vectorial form usage on the dependent variable x and the independent variable t hereonforth:
x'=f(t,x)
Which on this case is reduced to the trivial, but useless, set:
x(1)' = x(2)
x(2)' = -x(1)
This is solved as:
f=#(t,x)([-x(2);x(1)]);
y0=[1 0];
% f(0,y0) %Check IC = y'
[t, x] = ode23(f,[0 pi*2],y0);
plot(t,x)
However, if we need or want or we are forced to solve directly the system as is -i.e. implicit-, we should have the set defined as - again, g can be a vector of equations:
g(t,x,x')=0
by using ode15i - i standing for "implicit":
g=#(t,x,dx)([ 2*dx(1)+dx(2)-x(1)+2*x(2); ...
dx(1)+3*dx(2)-3*x(1)+x(2)]);
y0=[1;0];
dy0=[0;1]; % kind of magic to find...
% g(0,y0,dy0) %Check IC = 0
[t, x] = ode15i(g, [0 pi*2],y0,dy0);
plot(t,x)
The solution is the same for both options...
I'm using ode45 to solve second order differential equation. the time span is determined based on how many numbers in txt file, therefore, the time span is defined as follows
i = 1;
t(i) = 0;
dt = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
i = i + 1;
t(i) = t(i-1) + dt;
end
Now the time elements should not exceed the size of txt (i.e. numel(theta_d)). In main.m, I have
x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45('ODESolver', t, x0, options);
and ODESolver.m header is
function dx = ODESolver(t, x)
If I run the code, I'm getting this error
Attempted to access theta_d(56); index out of bounds because numel(theta_d)=55.
Error in ODESolver (line 29)
theta_dDot = ( theta_d(i) - theta_dPrev ) / dt;
Why the ode45 is not being fixed with the time span?
Edit: this is the entire code
main.m
clear all
clc
global error theta_d dt;
error = 0;
theta_d = load('trajectory.txt');
i = 1;
t(i) = 0;
dt = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
i = i + 1;
t(i) = t(i-1) + dt;
end
x0 = [pi/4; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45(#ODESolver, t, x0, options);
%e = x(:,1) - theta_d; % Error theta
plot(t, x(:,2), 'r', 'LineWidth', 2);
title('Tracking Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on
and ODESolver.m
function dx = ODESolver(t, x)
persistent i theta_dPrev
if isempty(i)
i = 1;
theta_dPrev = 0;
end
global error theta_d dt ;
dx = zeros(2,1);
%Parameters:
m = 0.5; % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1; % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81; % acceleration due to gravity m/s^2
% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;
% theta_d first derivative
theta_dDot = ( theta_d(i) - theta_dPrev ) / dt;
theta_dPrev = theta_d(i);
% u: joint torque
u = Kp*(theta_d(i) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));
dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));
i = i + 1;
end
and this is the error
Attempted to access theta_d(56); index out of bounds because numel(theta_d)=55.
Error in ODESolver (line 28)
theta_dDot = ( theta_d(i) - theta_dPrev ) / dt;
Error in ode45 (line 261)
f(:,2) = feval(odeFcn,t+hA(1),y+f*hB(:,1),odeArgs{:});
Error in main (line 21)
[t, x] = ode45(#ODESolver, t, x0, options);
The problem here is because you have data at discrete time points, but ode45 needs to be able to calculate the derivative at any time point in your time range. Once it solves the problem, it will interpolate the results back onto your desired time points. So it will calculate the derivative many times more than at just the time points you specified, thus your i counter will not work at all.
Since you have discrete data, the only way to proceed with ode45 is to interpolate theta_d to any time t. You have a list of values theta_d corresponding to times 0:dt:(dt*(numel(theta_d)-1)), so to interpolate to a particular time t, use interp1(0:dt:(dt*(numel(theta_d)-1)),theta_d,t), and I turned this into an anonymous function to give the interpolated value of theta_p at a given time t
Then your derivative function will look like
function dx = ODESolver(t, x,thetaI)
dx = zeros(2,1);
%Parameters:
m = 0.5; % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1; % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81; % acceleration due to gravity m/s^2
% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;
% theta_d first derivative
dt=1e-4;
theta_dDot = (thetaI(t) - theta(I-dt)) / dt;
%// Note thetaI(t) is the interpolated theta_d values at time t
% u: joint torque
u = Kp*(thetaI(t) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));
dx=[x(2); 1/I*(u - d*x(2) - m*g*L*sin(x(1)))];
end
and you will have to define thetaI=#(t) interp1(0:dt:(dt*(numel(theta_d)-1)),theta_d,t); before calling ode45 using [t, x] = ode45(#(t,x) ODESolver(t,x,thetaI, t, x0, options);.
I removed a few things from ODESolver and changed how the derivative was computed.
Note I can't test this, but it should get you on the way.
Got one more problem with matrix multiplication in Matlab. I have to plot Taylor polynomials for the given function. This question is similar to my previous one (but this time, the function is f: R^2 -> R^3) and I can't figure out how to make the matrices in order to make it work...
function example
clf;
M = 40;
N = 20;
% domain of f(x)
x1 = linspace(0,2*pi,M).'*ones(1,N);
x2 = ones(M,1)*linspace(0,2*pi,N);
[y1,y2,y3] = F(x1,x2);
mesh(y1,y2,y3,...
'facecolor','w',...
'edgecolor','k');
axis equal;
axis vis3d;
axis manual;
hold on
% point for our Taylor polynom
xx1 = 3;
xx2 = 0.5;
[yy1,yy2,yy3] = F(xx1,xx2);
% plots one discrete point
plot3(yy1,yy2,yy3,'ro');
[y1,y2,y3] = T1(xx1,xx2,x1,x2);
mesh(y1,y2,y3,...
'facecolor','w',...
'edgecolor','g');
% given function
function [y1,y2,y3] = F(x1,x2)
% constants
R=2; r=1;
y1 = (R+r*cos(x2)).*cos(x1);
y2 = (R+r*cos(x2)).*sin(x1);
y3 = r*sin(x2);
function [y1,y2,y3] = T1(xx1,xx2,x1,x2)
dy = [
-(R + r*cos(xx2))*sin(xx1) -r*cos(xx1)*sin(xx2)
(R + r*cos(xx2))*cos(xx1) -r*sin(xx1)*sin(xx2)
0 r*cos(xx2) ];
y = F(xx1, xx2) + dy.*[x1-xx1; x2-xx2];
function [y1,y2,y3] = T2(xx1,xx2,x1,x2)
% ?
I know that my code is full of mistakes (I just need to fix my T1 function). dy represents Jacobian matrix (total derivation of f(x) - I hope I got it right...). I am not sure how would the Hessian matrix in T2 look, by I hope I will figure it out, I'm just lost in Matlab...
edit: I tried to improve my formatting - here's my Jacobian matrix
[-(R + r*cos(xx2))*sin(xx1), -r*cos(xx1)*sin(xx2)...
(R + r*cos(xx2))*cos(xx1), -r*sin(xx1)*sin(xx2)...
0, r*cos(xx2)];
function [y1,y2,y3]=T1(xx1,xx2,x1,x2)
R=2; r=1;
%derivatives
y1dx1 = -(R + r * cos(xx2)) * sin(xx1);
y1dx2 = -r * cos(xx1) * sin(xx2);
y2dx1 = (R + r * cos(xx2)) * cos(xx1);
y2dx2 = -r * sin(xx1) * sin(xx2);
y3dx1 = 0;
y3dx2 = r * cos(xx2);
%T1
[f1, f2, f3] = F(xx1, xx2);
y1 = f1 + y1dx1*(x1-xx1) + y1dx2*(x2-xx2);
y2 = f2 + y2dx1*(x1-xx1) + y2dx2*(x2-xx2);
y3 = f3 + y3dx1*(x1-xx1) + y3dx2*(x2-xx2);