function Cb2n = rotz(theta)
% 3D初等旋转， theta为旋转角度，rad
Cb2n = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1];

end