Okay, this code seems to almost work. Unfortunately, I got it from a textbook. When I make a minor change in it to produce an increase in altitude. It craps out. Any idea what may be wrong?
pn=0;
pe=0;
pd=0;
phi=0;
theta=0;
psi=0;
handle=[];
%Simulation parameters
dpn=0.5; %Change in position.
dpsi=0.01; %Change in yaw angle.
simlength=100;%Number of Sim Steps
%Draw and update the plane's position.
for k = 1:simlength
pn = pn+dpn;
pe = pe+dpn;
%pd = pd +.1;
psi = psi+dpsi;
%Draw or update the plane.
handle = drawPlaneBody(pn,pe,pd,phi,theta,psi,handle);
pause(0.1); %Pause to visualize the movement
end
function handle = drawPlaneBody(pn,pe,pd,phi,theta,psi,handle)
%define points on plane in local NED coordintates
NED = airplanepoints;
%rotate plane by (phi; theta; psi)
NED = rotate(NED, phi, theta, psi);
%translate plane to [pn; pe; pd]
NED = translate(NED, pn, pe, pd);
%transform vertices from NED to XYZ
R = [...
0, 1, 0;...
1, 0 , 0;...
0, 0, -1;...
];
XYZ = R* NED';
%plot plane
if isempty(handle)
handle = plot3(XYZ(1,:),XYZ(2,:),XYZ(3,:),'b');
xlim([-100 100]);
ylim([-100 100]);
zlim([0 100]);
grid on;%show grid
xlabel('X');ylabel('Y');zlabel('Z');%label axes
else
set(handle, 'XData', XYZ(1,:), 'YData',XYZ(2,:), 'ZData',XYZ(3,:));
drawnow
end
end
function XYZ = airplanepoints
%define points on the aircraft in local NED
XYZ = [...
0 0 0;%point1
-2 1 1;%point2
-2 1 -1;%point3
0 0 0;%point1
-2 -1 1;%point4
-2 -1 -1;%point5
0 0 0;%point1
-2 1 1;%point2
-2 -1 1;%point4
0 0 0;%point1
-2 1 -1;%point3
-2 -1 -1;%point5
0 0 0;%point1
-2 1 1;%point2
-18 0 0;%point6
-2 1 -1;%point3
-18 0 0;%point6
-2 -1 -1;%point5
-18 0 0;%point6
-2 -1 1;%point4
-18 0 0;%point6
-2 1 1;%point2
0 0 0;%point1
-5 0 0;%point7
-5 -10 0;%point8
-8 -10 0;%point9
-8 10 0;%point10
-5 10 0;%point11
-5 0 0;%point7
-15.5 0 0;%point12
-15.5 2 0;%point13
-17.5 2 0;%point14
-17.5 -2 0;%point15
-15.5 -2 0;%point16
-15.5 0 0;%point12
-18 0 0;%point6
-18 0 -3;%point17
-15.5 0 0;%point15
-18 0 0;%point16
];
end
function XYZ=rotate(XYZ,phi,theta,psi)
%define rotation matrix
R_roll = [
1, 0, 0;
0, cos(phi), -sin(phi);
0, sin(phi), cos(phi)];
R_pitch = [
cos(theta), 0, sin(theta);
0, 1, 0;
-sin(theta), 0, cos(theta)];
R_yaw = [
cos(psi), -sin(psi), 0;
sin(psi), cos(psi), 0;
0, 0, 1];
R = R_roll*R_pitch*R_yaw;
%rotate vertices
XYZ =R*XYZ';
XYZ = XYZ';
end
function XYZ = translate(XYZ, pn, pe, pd)
XYZ = XYZ' + repmat([pn;pe;pd],1,size(XYZ,1));
XYZ=XYZ';
end
