Let $N$ a large number. Let $x,y \in \mathbb{R}^{2 \times N}$. Each column of these vectors is denoted with subindex $i$ : $x_i$ stands for the i-th column of $x$. Let $R$ be a $2 \times 2$ rotation matrix, and $\Delta \in \mathbb{R}^2$, such that: \begin{equation} R^Tx_i = y_i-\Delta \end{equation} In practice, there is noise $e_i \in \mathbb{R}^2$ on the signal $y_i$ : \begin{equation} R^Tx_i = (y_i+e_i)-\Delta \end{equation} Its covariance $\Sigma_i \in \mathbb{R}^{2 \times 2}$ is given at each time sample.

The least squares problem that we are interested in is: \begin{equation} \min_{R,\Delta} \frac{1}{N}\sum_{i = 1}^N \| \Sigma_i^{-1/2}( R^T x_i - y_i + \Delta)\|_2^2 \end{equation} to account for the uncertainty in each sample $y_i$ through $\Sigma_i$. How to solve for $R, \Delta$ ?


1 Answer 1


Since $N$ is a constant, we don't have to worry about it, and we'll just minimize

$f(R, \Delta) = \displaystyle \sum_{i=1}^N (R^T x_i - y_i + \Delta)^T \sigma_i (R^T x_i - y_i + \Delta) $

The gradient of $f(R, \Delta)$ with resepct to $\Delta$ is

$ \nabla_{\Delta} f = 2 \displaystyle \sum_{i=1}^N \sigma_i (R^T x_i - y_i + \Delta ) = 0 $

Here $\sigma_i$ is the weight of the $i$-th observation.

And this means that at the minimum we will have

$ \Delta = \displaystyle - \dfrac{ \sum_{i=1}^N \sigma_i (R^T x_i - y_i)}{\sum_{i=1}^N \sigma_i} = - \dfrac{1}{\sum_{i=1}^N \sigma_i} \cdot \left( R^T \left(\sum_{i=1}^N \sigma_i x_i\right) - \left(\sum_{i=1}^N \sigma_i y_i \right) \right)\\ = - \left( R^T x_{ave} - y_{ave} \right)$


$ x_{ave} = \dfrac{1}{\sum_{i=1}^N \sigma_i} \cdot \displaystyle \left( \sum_{i=1}^N \sigma_i x_i\right)$

$ y_{ave} = \dfrac{1}{\sum_{i=1}^N \sigma_i} \cdot \displaystyle \left(\sum_{i=1}^N \sigma_i y_i \right) $

Substituting the expression for $\Delta$ above into $f(R, \Delta) $ yields

$ f(R) = \displaystyle \sum_{i=1}^N (R^T z_i - w_i)^T \sigma_i (R^T z_i - w_i) $

where $z_i = x_i - x_{ave} $ and $ w_i = y_i - y_{ave} $

Expanding $f(R)$:

$f(R) = \displaystyle \sum_{i=1}^N \sigma_i \left( z_i^T R R^T z_i + w_i^T w_i - 2 z_i^T R w_i \right) = \sum_{i=1}^N \sigma_i \left( z_i^T z_i + w_i^T w_i - 2 z_i^T R w_i \right) $

The first two terms in the summand are constant and independent of $R$, so they can dropped from the objective function. Hence, we now want to minimize

$ \underset{R}{\text{min}} \hspace{10pt} \left(- 2 \displaystyle \sum_{i=1}^N \sigma_i z_i^T R w_i \right)$

Hence, we want to find

$ \underset{R}{\text{max}} \hspace{10pt} \left( \displaystyle \sum_{i=1}^N \sigma_i z_i^T R w_i \right)$


$ \sigma_i z_i^T R w_i = \text{trace}( \sigma_i w_i z_i^T R )$


$ \displaystyle \sum_{i=1}^N \left( \sigma_i z_i^T R w_i \right) = \text{trace}( W \Sigma Z^T R) $

where $W \in \mathbb{R}^{2 \times N} $ has its $i$-th column equal to $w_i$ and $ Z \in \mathbb{R}^{2 \times N} $ has its $i$-th column equal to $z_i$, and $\Sigma \in \mathbb{R}^{N \times N} $ is diagonal with the $i$-th diagonal element equal to $\sigma_i $.

At this point, we can find the SVD (Singular Value Decomposition) of the matrix $ W \Sigma Z^T $ , so that

$ W \Sigma Z^T = U S V^T $

here $S$ is the diagonal matrix of the singular values resulting from the decomposition, and $U$ and $V$ are unitary matrices (i.e. they are rotation matrices). Now we have

$ \text{trace}( U S V^T R ) = \text{trace} ( S V^T R U ) $

Note that $V^T R U$ is a rotation matrix, and thus the trace will be maximum where $V^T R U$ is equal to the identity matrix. Therefore for the maximum trace we must have

$ V^T R U = I $


$ R = V U^T $

Once we have $R$, we can calculate $ \Delta = - \left( R^T x_{ave} - y_{ave} \right)$

  • $\begingroup$ Many thanks for these explanations. However, I have doubts left : isn't it that the weights are just going to the S matrix when computing the SVD, and that whatever the matrix $\Sigma$, the result will be the same for $U, V$ ? Hence, on one side, the weights are taken into account in the $x_{ave},y_{ave}$, but not for the SVD result ? Or am I missing something ? I am a little bit puzzled when setting $\sigma_i = \sigma$ for all $i$. $\endgroup$
    – baptiste
    Mar 4, 2023 at 13:49
  • $\begingroup$ You're right. There was an error in the expression for $\Delta$, and $x_{ave}$ and $y_{ave}$. It is corrected now. $\endgroup$
    – of course
    Mar 4, 2023 at 15:35
  • $\begingroup$ This clears ideas a lot ! :) $\endgroup$
    – baptiste
    Mar 4, 2023 at 18:35

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .