function [A, B, C, D] = pvtol_linearize(xd, ud)

pvtol_params;				% load system parameters

A = [  0     0     0     1     0     0;
       0     0     0     0     1     0;
       0     0     0     0     0     1;
       0, 0, (-ud(1)*sin(xd(3)) - ud(2)*cos(xd(3)))/m, -c/m, 0, 0;
       0, 0, (ud(1)*cos(xd(3)) - ud(2)*sin(xd(3)))/m, 0, -c/m, 0;
       0     0     0     0     0     0 ];

B = [0 0; 0 0; 0 0;
  cos(xd(3))/m, -sin(xd(3))/m;
  sin(xd(3))/m,  cos(xd(3))/m;
  r/J, 0 ];

C = [1 0 0 0 0 0; 0 1 0 0 0 0];
D = [0 0; 0 0];
