00001 function dy = steeringEst(t,y)
00002
00003 % v1i = speed
00004 v1i = 1;
00005 % v2i = steering target angle = x * 20 - angle
00006 v2i = -0.5 * y(2) - 0.7 * y(3);
00007 if (v2i > pi/6)
00008 v2i = pi/6;
00009 end
00010 if (v2i < -pi/6)
00011 v2i = -pi/6;
00012 end
00013
00014
00015 % length of truck (in meters) from back axle to front axle
00016 truckLength = 0.3048;
00017
00018 % y(1) = x
00019 % y(2) = y
00020 % y(3) = theta (truck angle)
00021 % y(4) = steering angle
00022
00023 dy = zeros(4,1);
00024 dy(1) = cos(y(3)) * v1i;
00025 dy(2) = sin(y(3)) * v1i;
00026 dy(3) = tan(y(4)) / truckLength;
00027 dy(4) = 2.0 * (v2i - y(4)) * v1i;
