rdots = [vGx; vGy; omega];
rddots = [aGx; aGy; omegadot];
J11 = ( F1( (xG+h), yG, theta, vGx, vGy, omega ) - F1( (xG-h), yG, theta, vGx, vGy, omega ) ) / 2*h;
J12 = ( F1( xG, (yG+h), theta, vGx, vGy, omega ) - F1( xG, (yG -h), theta, vGx, vGy, omega ) ) / 2*h;
J13 = ( F1( xG, yG, (theta+h), vGx, vGy, omega ) - F1( xG, yG, (theta-h), vGx, vGy, omega ) ) / 2*h;
J14 = ( F1( xG, yG, theta, (vGx+h), vGy, omega ) - F1( xG, yG, theta, (vGx-h), vGy, omega ) ) / 2*h;
J15 = ( F1( xG, yG, theta, vGx, (vGy+h), omega ) - F1( xG, yG, theta, vGx, (vGy-h), omega ) ) / 2*h;
J16 = ( F1( xG, yG, theta, vGx, vGy, (omega+h) ) - F1( xG, yG, theta, vGx, vGy, (omega-h) ) ) / 2*h;
J21 = ( F2( (xG+h), yG, theta, vGx, vGy, omega ) - F2( (xG-h), yG, theta, vGx, vGy, omega ) ) / 2*h;
J22 = ( F2( xG, (yG+h), theta, vGx, vGy, omega ) - F2( xG, (yG -h), theta, vGx, vGy, omega ) ) / 2*h;
J23 = ( F2( xG, yG, (theta+h), vGx, vGy, omega ) - F2( xG, yG, (theta-h), vGx, vGy, omega ) ) / 2*h;
J24 = ( F2( xG, yG, theta, (vGx+h), vGy, omega ) - F2( xG, yG, theta, (vGx-h), vGy, omega ) ) / 2*h;
J25 = ( F2( xG, yG, theta, vGx, (vGy+h), omega ) - F2( xG, yG, theta, vGx, (vGy-h), omega ) ) / 2*h;
J26 = ( F2( xG, yG, theta, vGx, vGy, (omega+h) ) - F2( xG, yG, theta, vGx, vGy, (omega-h) ) ) / 2*h;
J31 = ( F3( (xG+h), yG, theta, vGx, vGy, omega ) - F3( (xG-h), yG, theta, vGx, vGy, omega ) ) / 2*h;
J32 = ( F3( xG, (yG+h), theta, vGx, vGy, omega ) - F3( xG, (yG -h), theta, vGx, vGy, omega ) ) / 2*h;
J33 = ( F3( xG, yG, (theta+h), vGx, vGy, omega ) - F3( xG, yG, (theta-h), vGx, vGy, omega ) ) / 2*h;
J34 = ( F3( xG, yG, theta, (vGx+h), vGy, omega ) - F3( xG, yG, theta, (vGx-h), vGy, omega ) ) / 2*h;
J35 = ( F3( xG, yG, theta, vGx, (vGy+h), omega ) - F3( xG, yG, theta, vGx, (vGy-h), omega ) ) / 2*h;
J36 = ( F3( xG, yG, theta, vGx, vGy, (omega+h) ) - F3( xG, yG, theta, vGx, vGy, (omega-h) ) ) / 2*h;
J41 = ( F4( (xG+h), yG, theta, vGx, vGy, omega ) - F4( (xG-h), yG, theta, vGx, vGy, omega ) ) / 2*h;
J42 = ( F4( xG, (yG+h), theta, vGx, vGy, omega ) - F4( xG, (yG -h), theta, vGx, vGy, omega ) ) / 2*h;
J43 = ( F4( xG, yG, (theta+h), vGx, vGy, omega ) - F4( xG, yG, (theta-h), vGx, vGy, omega ) ) / 2*h;
J44 = ( F4( xG, yG, theta, (vGx+h), vGy, omega ) - F4( xG, yG, theta, (vGx-h), vGy, omega ) ) / 2*h;
J45 = ( F4( xG, yG, theta, vGx, (vGy+h), omega ) - F4( xG, yG, theta, vGx, (vGy-h), omega ) ) / 2*h;
J46 = ( F4( xG, yG, theta, vGx, vGy, (omega+h) ) - F4( xG, yG, theta, vGx, vGy, (omega-h) ) ) / 2*h;
J51 = ( F5( (xG+h), yG, theta, vGx, vGy, omega ) - F5( (xG-h), yG, theta, vGx, vGy, omega ) ) / 2*h;
J52 = ( F5( xG, (yG+h), theta, vGx, vGy, omega ) - F5( xG, (yG -h), theta, vGx, vGy, omega ) ) / 2*h;
J53 = ( F5( xG, yG, (theta+h), vGx, vGy, omega ) - F5( xG, yG, (theta-h), vGx, vGy, omega ) ) / 2*h;
J54 = ( F5( xG, yG, theta, (vGx+h), vGy, omega ) - F5( xG, yG, theta, (vGx-h), vGy, omega ) ) / 2*h;
J55 = ( F5( xG, yG, theta, vGx, (vGy+h), omega ) - F5( xG, yG, theta, vGx, (vGy-h), omega ) ) / 2*h;
J56 = ( F5( xG, yG, theta, vGx, vGy, (omega+h) ) - F5( xG, yG, theta, vGx, vGy, (omega-h) ) ) / 2*h;
J61 = ( F6( (xG+h), yG, theta, vGx, vGy, omega ) - F6( (xG-h), yG, theta, vGx, vGy, omega ) ) / 2*h;
J62 = ( F6( xG, (yG+h), theta, vGx, vGy, omega ) - F6( xG, (yG -h), theta, vGx, vGy, omega ) ) / 2*h;
J63 = ( F6( xG, yG, (theta+h), vGx, vGy, omega ) - F6( xG, yG, (theta-h), vGx, vGy, omega ) ) / 2*h;
J64 = ( F6( xG, yG, theta, (vGx+h), vGy, omega ) - F6( xG, yG, theta, (vGx-h), vGy, omega ) ) / 2*h;
J65 = ( F6( xG, yG, theta, vGx, (vGy+h), omega ) - F6( xG, yG, theta, vGx, (vGy-h), omega ) ) / 2*h;
J66 = ( F6( xG, yG, theta, vGx, vGy, (omega+h) ) - F6( xG, yG, theta, vGx, vGy, (omega-h) ) ) / 2*h;
J = [ J11, J12, J13, J14, J15, J16;
J21, J22, J23, J24, J25, J26;
J31, J32, J33, J34, J35, J36;
J41, J42, J43, J44, J45, J46;
J51, J52, J53, J54, J55, J56;
J61, J62, J63, J64, J65, J66 ];