这篇文章介绍了平面轨迹对齐的原理和推导，并附上代码。
问题引入
在评估惯导推算轨迹精度时，需要将惯导轨迹与参考轨迹对齐。
$$
\begin{split}
\begin{bmatrix} x_{ref} \ y_{ref} \end{bmatrix} = &
\begin{bmatrix} l\cdot sin(\psi_{ref}) \ l \cdot cos(\psi_{ref}) \end{bmatrix} =
\begin{bmatrix} l\cdot sin(\psi_1 - \Delta \psi) \ l \cdot cos(\psi_1 - \Delta \psi) \end{bmatrix} =
\begin{bmatrix} l\cdot(sin\psi_1cos\Delta\psi - cos\psi_1 sin\Delta\psi) \ l\cdot(cos\psi_1cos\Delta\psi + sin\psi_1 sin\Delta\psi) \end{bmatrix} \ = &
\begin{bmatrix} x_1 \cdot cos\Delta\psi - y_1 \cdot sin\Delta\psi \ x_1 \cdot sin\Delta\psi - y_1 \cdot cos\Delta\psi \end{bmatrix} =
\begin{bmatrix} cos\Delta\psi & -sin\Delta\psi \ sin\Delta\psi & cos\Delta\psi \end{bmatrix} \begin{bmatrix} x_1 \ y_1 \end{bmatrix}
\end{split}
$$