# Estimate Euler angles between rotated coordinate system via Newton-method based on position vectors

I've got $N$ position vectors $\mathbf{a}_i = \begin{pmatrix} a_{i,x} \\ a_{i,y} \\ a_{i,z} \end{pmatrix}$ in one coordinate system and $N$ corresponding position vectors $\mathbf{b}_i = \begin{pmatrix} b_{i,x} \\ b_{i,y} \\ b_{i,z} \end{pmatrix}$ in a rotated coordinate system. With $\mathbf{A} = \begin{pmatrix} \mathbf{a}_1 ... \mathbf{a}_M\end{pmatrix}$ and $\mathbf{B} = \begin{pmatrix} \mathbf{b}_1 ... \mathbf{b}_M\end{pmatrix}$ and $\mathbf{M}(\phi,\theta,\psi) = \mathbf{M}_{\mathrm{yaw}}(\phi)\mathbf{M}_{\mathrm{pitch}}(\theta)\mathbf{M}_{\mathrm{roll}}(\psi)$, $\mathbf{M} \in \mathbb{R}^{3 \times 3}$ corresponding to the euler rotation matrix yaw, pitch and roll, the following holds $\mathbf{A} \approx \mathbf{M}(\phi,\theta,\psi)\mathbf{B}$ which leads to the minimisation problem $$\min_{\phi,\theta,\psi} \|\mathbf{A}-\mathbf{M}(\phi,\theta,\psi)\mathbf{B}\|_2^2$$ using the Newton-method $\mathbf{F}(\phi,\theta,\psi) = \mathbf{A}-\mathbf{M}(\phi,\theta,\psi)\mathbf{B}$ with $\mathbf{F} : \mathbb{R}^3 \rightarrow \mathbb{R}^{3 \times N}$

The Newton-Method requires a Taylor series to the first order and therefore $\mathbf{F}'$. But how do you build a Jacobi-matrix of a $\mathbb{R}^{3 \times N}$-matrix?

It can be solved by rewriting the matrix to a vector: $$\mathbf{c} = \begin{pmatrix} a_{1,x} \\ a_{1,y} \\ a_{1,z} \\a_{2,x} \\ a_{2,y} \\ a_{2,z} \\ ..\end{pmatrix}, \mathbf{d}(\phi,\theta,\psi) = \begin{pmatrix} M_{1,x}b_{1,x}+M_{2,x}b_{1,y}+M_{3,x}b_{1,z} \\ M_{1,y}b_{1,x}+M_{2,y}b_{1,y}+M_{3,y}b_{1,z} \\ M_{1,z}b_{1,x}+M_{2,z}b_{1,y}+M_{3,z}b_{1,z} \\ M_{1,x}b_{2,x}+M_{2,x}b_{2,y}+M_{3,x}b_{2,z} \\ M_{1,y}b_{2,x}+M_{2,y}b_{2,y}+M_{3,y}b_{2,z} \\ M_{1,z}b_{2,x}+M_{2,z}b_{2,y}+M_{3,z}b_{2,z} \\ .. \end{pmatrix}$$ i.e. $\mathbf{A} \in \mathbb{R}^{3 \times 3} \rightarrow \mathbf{c} \in \mathbb{R}^9$ and $\mathbf{M}(\phi,\theta,\psi)\mathbf{B} \in \mathbb{R}^{3 \times 3} \rightarrow \mathbf{d}(\phi,\theta,\psi) \in \mathbb{R}^9$ $$\Rightarrow \min_{\phi,\theta,\psi} \|\mathbf{A}-\mathbf{M}(\phi,\theta,\psi)\mathbf{B}\|_2^2 \Leftrightarrow \min_{\phi,\theta,\psi} \|\mathbf{c}-\mathbf{d}(\phi,\theta,\psi)\|_2^2$$ The Taylor series to the first order of $\mathbf{d}(\phi,\theta,\psi)$ can be derived by $\mathbf{d}(\phi,\theta,\psi) = \mathbf{d}(\phi_k,\theta_k,\psi_k) + \underbrace{\mathbf{J}(\mathbf{d}(\phi_k,\theta_k,\psi_k))}_{=\mathbf{G}} \underbrace{\begin{pmatrix} \phi - \phi_k \\ \theta - \theta_k \\ \psi - \psi_k \end{pmatrix}}_{=\Delta\mathbf{\varphi}_k}$ where $\mathbf{J}(\mathbf{d})$ is the Jacobi-matrix of $\mathbf{d}$, $\mathbf{G} \in \mathbb{R}^{3N \times 3}$ and $\Delta\mathbf{\varphi}_k \in \mathbb{R}^3$
$\Delta\mathbf{\varphi}_k$ can then be calculated by $$\Delta\mathbf{\varphi}_k = (\mathbf{G}^T\mathbf{G})^{-1}\mathbf{G}^T(\mathbf{c}-\mathbf{d}(\phi_k,\theta_k,\psi_k))$$ the next iterative step is $$\mathbf{\varphi}_{k+1} = \mathbf{\varphi}_{k} + \Delta\mathbf{\varphi}_k$$ The stop criterion can be $\|\Delta\mathbf{\varphi}_k\| < \epsilon$ and start value could be $\mathbf{\varphi}_0 = \begin{pmatrix} 0\\0\\0 \end{pmatrix}$