[ x ′ y ′ z ′ ] = [ x y z ] [ c o s θ s i n θ 0 − s i n θ c o s θ 0 0 0 1 ] \begin{bmatrix} x'&y'&z' \end{bmatrix}=\begin{bmatrix} x&y&z \end{bmatrix}\begin{bmatrix} \mathrm{cos}\ \theta &\mathrm{sin}\ \theta &0 \\ \mathrm{-sin}\ \theta &\mathrm{cos}\ \theta &0 \\ 0&0&1 \end{bmatrix} [x′y′z′]=[xyz] cos θ−sin θ0sin θcos θ0001
其中 θ \theta θ使车辆的朝角（heading）。
同理，如果将世界坐标系转换到主车坐标系，公式如下：
[ x y z ] = [ x ′ y ′ z ′ ] [ c o s θ − s i n θ 0 s i n θ c o s θ 0 0 0 1 ] \begin{bmatrix} x&y&z \end{bmatrix}=\begin{bmatrix} x'&y'&z' \end{bmatrix}\begin{bmatrix} \mathrm{cos}\ \theta &\mathrm{-sin}\ \theta &0 \\ \mathrm{sin}\ \theta &\mathrm{cos}\ \theta &0 \\ 0&0&1 \end{bmatrix} [xyz]=[x′y′z′] cos θsin θ0−sin θcos θ0001