045demo_cone_motion

圆锥运动仿真
工具箱版本：psins140410

---

qnb = [cos(afa/2); sin(afa/2)*cos(w*t); sin(afa/2)*sin(w*t); 0];

变换四元数，参考严书(2.6-2)。

---

x = qmulv(qnb,[1;0;0]);    
y = qmulv(qnb,[0;1;0]);    
z = qmulv(qnb,[0;0;1]);

三个坐标轴上的矢量分别按照变换四元数进行旋转后得到的矢量。

---

if t<2*pi/f
    xk(ki,:) = x'; yk(ki,:) = y'; zk(ki,:) = z';
end

记录一个周期的旋转矢量数据，为的是后面绘制动坐标系的旋转轨迹。

---

if ~ishandle(hfig)
    break; 
end

ishandle: 若是图形句柄则为真;
如果hfig是图形句柄，为1，取反为假，不跳出。

---

clf(hfig);

清除图对象

---

plot3([0,1.5], [0,0], [0,0],'m', ...
      [0,0], [0,1.5], [0,0],'m', ...
      [0,0], [0,0], [0,1.5], 'm');

根据矢量，具体可参考044关于matlab的绘图函数plot，此处为同时画三个矢量，作为一个固定的坐标系。

---

view([1 1.2 1.1]);

从(1,1.2,1.1)看向原点的角度。

---

plot3([0,x(1)],[0,x(2)],[0,x(3)],'-o', ...
          [0,y(1)],[0,y(2)],[0,y(3)],'-o', ...
          [0,z(1)],[0,z(2)],[0,z(3)],'-o','LineWidth',3);

绘制动坐标系。

---

plot3(xk(:,1),xk(:,2),xk(:,3), ...
          yk(:,1),yk(:,2),yk(:,3), ...
          zk(:,1),zk(:,2),zk(:,3)); 

根据前面记录的一个周期的轨迹，进行动坐标系运动轨迹的绘制。

---

pause(.2);

必须要有，不然你是看不到图了。