solving rotation matrix angles using Gauss-Newton

187 Views Asked by At

I am attempting estimate the three angles of a rotation matrix that has been applied to a known set of vectors using the Gauss-Newton method through direct observation the rotated vectors that have been corrupted by noise. Given a set $\textrm{V}$ of $N$ known vectors

\begin{equation*} \textrm{V} = \begin{bmatrix} x_1 & & x_N\\ y_1 & \cdots & y_N\\ z_1 & & z_N \end{bmatrix} \end{equation*}

with an applied rotation matrix $\textrm{R}$, \begin{align*}\label{Rx} \textrm{R}_{x} = \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos{\theta_{x}} & \sin{\theta_{x}}\\ 0 & \sin{\theta_{x}} & \cos{\theta_{x}} \end{bmatrix} & \textrm{R}_{y} = \begin{bmatrix} \cos{\theta_{y}} & 0 & -\sin{\theta_{y}}\\ 0 & 1 & 0\\ \sin{\theta_{y}} & 0 & \cos{\theta_{y}} \end{bmatrix} & \textrm{R}_{z} = \begin{bmatrix} \cos{\theta_{z}} & -\sin{\theta_{z}} & 0\\ \sin{\theta_{z}} & \cos{\theta_{z}} & 0\\ 0 & 0 & 1 \end{bmatrix} \end{align*}

\begin{eqnarray*}\label{R} \textrm{R} & = & \textrm{R}_z\textrm{R}_y\textrm{R}_x\\ & = & \begin{bmatrix} \cos\theta_y \cos\theta_z & \cos\theta_z \sin\theta_x \sin\theta_y - \cos\theta_x \sin\theta_z & \cos\theta_x \cos\theta_z \sin\theta_y + \sin\theta_x \sin\theta_z\\ \cos\theta_y \sin\theta_z & \cos\theta_x \cos\theta_z + \sin\theta_x \sin\theta_y \sin\theta_z& -\cos\theta_z \sin\theta_x + \cos\theta_x \sin\theta_y \sin\theta_z \\ -\sin\theta_y & \cos\theta_y \sin\theta_x & \cos\theta_x \cos\theta_y \end{bmatrix} \end{eqnarray*}

resulting rotated vectors $\textrm{W}$ are observed with noise $\eta$. \begin{equation*} \textrm{W} = \textrm{R}\textrm{V} + \eta \end{equation*}

Ideally, the $n^{th}$ column of $\textrm{W}$ is calculated as

\begin{equation*}\label{W} \textbf{w}= \begin{bmatrix} x_n \cos\theta_y \cos\theta_z + y_n ( \sin\theta_x \sin\theta_y\cos\theta_z - \cos\theta_x \sin\theta_z) + z_n (\cos\theta_x \sin\theta_y\cos\theta_z + \sin\theta_x \sin\theta_z)\\ x_n \cos\theta_y \sin\theta_z + y_n (\cos\theta_x \cos\theta_z + \sin\theta_x \sin\theta_y \sin\theta_z) + z_n (-\sin\theta_x \cos\theta_z + \cos\theta_x \sin\theta_y \sin\theta_z) \\ -x_n \sin\theta_y+ y_n \sin\theta_x \cos\theta_y+ z_n \cos\theta_x \cos\theta_y \end{bmatrix} \end{equation*}

Attempting to estimate the state vector $\beta$, the Jacobian is calculated as follows \begin{equation*} \beta = \begin{bmatrix} \theta_x \\ \theta_y \\ \theta_z \end{bmatrix} \end{equation*}

\begin{equation*} \textrm{J}_n (\beta)= \begin{bmatrix} j_{11} & j_{12} & j_{13}\\ j_{21} & j_{22} & j_{23}\\ j_{31} & j_{32} & j_{33}\\ \end{bmatrix} = \begin{bmatrix} \frac{\partial \textbf{w}_x}{\partial \theta_x} & \frac{\partial \textbf{w}_x}{\partial \theta_y} & \frac{\partial \textbf{w}_x}{\partial \theta_z}\\ \frac{\partial \textbf{w}_y}{\partial \theta_x} & \frac{\partial \textbf{w}_y}{\partial \theta_y} & \frac{\partial \textbf{w}_y}{\partial \theta_z}\\ \frac{\partial \textbf{w}_z}{\partial \theta_x} & \frac{\partial \textbf{w}_z}{\partial \theta_y} & \frac{\partial \textbf{w}_z}{\partial \theta_z} \end{bmatrix} \end{equation*}

\begin{eqnarray*} j_{11} & = & y_n (\cos\theta_x \sin\theta_y \cos\theta_z + \sin\theta_x \sin\theta_z) + z_n (-\sin\theta_x \sin\theta_y \cos\theta_z + \cos\theta_x \sin\theta_z)\\ j_{12} & = & - x_n \sin\theta_y \cos\theta_z + y_n \sin\theta_x\cos\theta_y \cos\theta_z + z_n\cos\theta_x \cos\theta_y \cos\theta_z \\ j_{13}& = &-x_n\cos\theta_y \sin\theta_z + y_n(- \sin\theta_x \sin\theta_y \sin\theta_z-\cos\theta_x \cos\theta_z ) + z_n( - \cos\theta_x \sin\theta_y \sin\theta_z+\sin\theta_x \cos\theta_z) \\ j_{21} & = &y_n(-\sin\theta_x \cos\theta_z + \cos\theta_x \sin\theta_y \sin\theta_z) + z_n(-\cos\theta_x \cos\theta_z - \sin\theta_x \sin\theta_y \sin\theta_z)\\ j_{22} & = & -x_n\sin\theta_y \sin\theta_z + y_n\sin\theta_x\cos\theta_y \sin\theta_z +z_n\cos\theta_x \cos\theta_y \sin\theta_z \\ j_{23} & = & x_n\cos\theta_y \cos\theta_z + y_n(-\cos\theta_x \sin\theta_z + \sin\theta_x \sin\theta_y \cos\theta_z ) + z_n(\sin\theta_x \sin\theta_z + \cos\theta_x\sin\theta_y \cos\theta_z)\\ j_{31} & = & y_n\cos\theta_x \cos\theta_y - z_n \sin\theta_x\cos\theta_y\\ j_{32} & = & -x_n\cos\theta_y - y_n\sin\theta_x \sin\theta_y - z_n\cos\theta_x \sin\theta_y \\ j_{33} & = & 0 \end{eqnarray*}

Residuals $\textbf{r}$ are defined as \begin{equation*} \textbf{r} = \textrm{W} - \textrm{V} = \begin{bmatrix} r_{1,1} & \cdots & r_{1,N}\\ r_{2,1} & \cdots & r_{2,N}\\ r_{3,1} & \cdots & r_{3,N} \end{bmatrix} \end{equation*}

Updating $\beta$ using the Gauss-Newton method is then implemented as follows \begin{align*} \underset{3N\times 3}{\textrm{J}(\beta)} = \begin{bmatrix} \textrm{J}_1({\beta})\\ \vdots\\ \textrm{J}_N({\beta}) \end{bmatrix} & \ & \underset{3N\times 1}{\textbf{r}}= \begin{bmatrix} r_{1,1}\\ r_{2,1}\\ r_{3,1}\\ \vdots\\ r_{1,N}\\ r_{2,N}\\ r_{3,N}\\ \end{bmatrix} \end{align*}

\begin{equation*} \beta_k = \beta_{k-1} - \left(\textrm{J}^T\textrm{J} \right)^{-1}\textrm{J}^T\textbf{r} \end{equation*}

This procedure does not yield the correct solution. Is there a reason the Gauss Newton method does not work in this situation? I am pretty sure the Jacobian is correct since it was computed in Mathematica and then verified by hand. Is there something I am overlooking here?