x=forwardk(1,4);%x-position symbole4 means row4 in forwardk 1means take the first value meansX
y=forwardk(2,4);%y-position
z=forwardk(3,4);%z-position
xr=(forwardk(1,1)+forwardk(1,2)+forwardk(1,3)) %the x componenet of the orientation vector
yr=(forwardk(2,1)+forwardk(2,2)+forwardk(2,3)) %the y componenet of the orientation vector
zr=(forwardk(3,1)+forwardk(3,2)+forwardk(3,3)) %the z componenet of the orientation vector
end
end
x=forwardk(1,4);%x-position symbole4 means row4 in forwardk 1means take the first value meansX
y=forwardk(2,4);%y-position
z=forwardk(3,4);%z-position
xr=(forwardk(1,1)+forwardk(1,2)+forwardk(1,3)) %the x componenet of the orientation vector
yr=(forwardk(2,1)+forwardk(2,2)+forwardk(2,3)) %the y componenet of the orientation vector
zr=(forwardk(3,1)+forwardk(3,2)+forwardk(3,3)) %the z componenet of the orientation vector
end
end