function f = uni_flat_care()
% EN.530.678: HW#3 sample
% 1) compute a reference path using a polynomial in flat output space
% 2) track the path using the the linearized dynamics
%
% M. Kobilarov, Spring 2014
% boundary conditions in state space
x0 = [-5; -3; 0.5];
xf = [0; 0; 1];
T = 10;
%%%%%%%%% TRAJECTORY GENERATION %%%%%%%%%%%%%
% norm of initial and final velocity along desired path
% it determines how much the curves will bend
% and can be freely chosen
S.u1 = 1;
% boundary conditions in flat output space
y0 = uni_h(x0);
yf = uni_h(xf);
dy0 = S.u1*[cos(x0(3)); sin(x0(3))]; % desired starting velocity
dyf = S.u1*[cos(xf(3)); sin(xf(3))]; % desired end velocity
% compute path coefficients
A = poly3_coeff(y0, dy0, yf, dyf, T);
% plot desired path
X = A*poly3([0:.01:T]);
plot(X(1,:), X(2,:), '-r')
hold on
%%%%%%%%% TRAJECTORY TRACKING %%%%%%%%%%%%%
S.A = A;
% perturb initial condition
x = x0 + [.25;.25;.1]
% simulate system
[ts, xs] = ode45(@uni_ode, [0 T], x, [], S);
% visualize
plot(xs(:,1), xs(:,2), '-b');
legend('desired', 'executed')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function A = poly3_coeff(y0, dy0, yf, dyf, T)
% computes cubic curve connecting (y0,dy0) and (yf, dyf) at time T
Y = [y0, dy0, yf, dyf];
L = [poly3(0), dpoly3(0), poly3(T), dpoly3(T)]
A = Y*inv(L);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function y = uni_h(x)
% output function
y = x(1:2);
function f = poly3(t)
f = [t.^3; t.^2; t; ones(size(t))];
function f = dpoly3(t)
f = [3*t.^2; 2*t; ones(size(t)); zeros(size(t))];
function f = d2poly3(t)
f = [6*t; 2; zeros(size(t)); zeros(size(t))];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u = uni_ctrl(t, x, S)
% tracking control law
% get desired outputs (x-y positions, velocities, accelerations)
yd = S.A*poly3(t);
dyd = S.A*dpoly3(t);
d2yd = S.A*d2poly3(t);
% desired angle
x3d = atan2(dyd(2), dyd(1));
xd = [yd; x3d];
% desired inputs
ud = [norm(dyd);
(d2yd(2)*dyd(1) - d2yd(1)*dyd(2))/norm(dyd)^2;]
% linearization, i.e. \dot (x - xd) = A(x - xd) + B(u - ud)
A = [0 0 -sin(x3d);
0 0 cos(x3d);
0 0 0]*ud(1);
B = [cos(x3d) 0;
sin(x3d) 0;
0 1];
% compute gain matrix for linear system (this solves the Lyapunov
% equation for the linearization dynamics)
[X,L,K] = care(A, B, eye(3));
% set control law
u = ud - K*(x - xd);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function dq = uni_ode(t, q, S)
% unicycle ODE
u = uni_ctrl(t, q, S);
dq = [cos(q(3))*u(1);
sin(q(3))*u(1)
u(2)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%