# Determining a family of Solutions to an underconstrained Modified Wahba's Problem, $J = \sum_{i=1}^{N} \frac{1}{2} \|a_i - RQR^Tb_i \|^2$

I have a minimisation problem of the form:

$$J(R) = \underset{R}{\mathrm{argmin}} \sum_{i=1}^{N} \frac{1}{2}\|a_i - RQR^T b_i \|^2$$

Where $$Q\in SO\{3\}$$ and $$a_i, b_i \in \mathbb{R}^3$$ are known, and we wish to determine $$R\in SO\{3\}$$.

I am specifically interested in the family or families of solutions for $$N=1$$, but I am yet to gain any specific insight. I have documented the approaches I have made below.

Approach 1:

For the standard Wahba's problem with cost function $$J = \sum_{i=1}^{N} \|a - Rb \|^2$$, both the uniquely determined case and undetermined case are well explored by Markley.

For $$N \ge 2$$, then if we let $$B=RQR^T$$, then $$B$$ can be uniquely determined via the standard Wahba's problem. We can then then relate:

$$B = RQR^T$$

Which is a matrix similarity problem. By exploring eigenvector properties, A family of solutions can then be computed from the underdetermined Wahba's problem:

$$u_B = Ru_Q$$

Where $$u_B$$ and $$u_Q$$ are the axes or rotation of $$B$$ and $$Q$$ respectively.

Unfortunately, this approach does not seem to work for $$N = 1$$; numerical experiments so far have failed when attempting to find a family of solutions in this way - $$B$$ is not uniquely constrained and somehow interacts with this second step in a way that I do not yet understand.

References

Markley, F. L. Attitude Determination using Vector Observations and the Singular Value Decomposition, Journal of the Astronautical Sciences, 1988, 38:245–258

When $$N=1$$, a global minimiser can be obtained explicitly. The objective function in this case is equal to $$\frac12(\|a\|^2+\|b\|^2)-a^TRQR^Tb$$ and the problem boils down to maximising $$\langle a,Q'b\rangle$$ where $$Q'=RQR^T$$.
If $$a=0$$ or $$b=0$$, every $$R$$ gives an optimal solution. Suppose $$a$$ and $$b$$ are nonzero. Normalise them to unit vectors and let $$\phi\in[0,\pi]$$ be the angle between them. That is, we assume that $$\|a\|=\|b\|=1$$ and $$\phi=\arccos\langle a,b\rangle\in[0,\pi]$$. Since $$Q$$ is a rotation matrix and $$Q'$$ is orthogonally similar to $$Q$$, the two matrices must have the same angles of rotation. Therefore $$Q=V\pmatrix{\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1}V^T \ \text{ and }\ Q'=U\pmatrix{\cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1}U^T$$ for some $$\theta\in[0,\pi]$$ and some rotation matrices $$U$$ and $$V$$ whose last columns are the rotation axes of $$Q$$ and $$Q'$$ respectively. Once $$Q'$$ is determined, we have $$Q'=RQR^T$$ by taking $$R=UV^T$$.
We now consider three possibilities. First, when $$\phi=0$$, $$a$$ is equal to $$b$$. Therefore $$\langle a,Q'b\rangle$$ is maximised and its value is $$1$$ when the rotation axis of $$Q'$$ is $$a$$. Hence we can pick a $$U\in SO(3)$$ whose last column is $$a$$.
Second, when $$\phi\ge\theta>0$$, since the great circle distance satisfies the triangle inequality, no rotation of $$b$$ by the angle $$\theta$$ about any axis can reduce the angle between $$a$$ and $$b$$ to a value smaller than $$\phi-\theta$$. Hence the maximum possible value of $$\langle a,Q'b\rangle$$ is $$\cos(\phi-\theta)$$ and this is attained when both $$a$$ and $$b$$ lie on the plane of rotation and $$Q'$$ rotates $$b$$ towards $$a$$. So, we can pick a rotation matrix $$U$$ whose last column is $$\frac{b\times a}{\|b\times a\|}$$ if $$b\ne-a$$, or any unit vector orthogonal to $$a$$ if $$b=-a$$.
Finally, when $$0<\phi<\theta$$, by picking an appropriate axis, one can rotate the vector $$b$$ by an angle $$\theta$$ to align with $$a$$. Therefore the maximum value of $$\langle a,RQR^Tb\rangle$$ is $$1$$ and it is attained when $$RQR^Tb=a$$. For example, let $$x=\frac{b-a}{\|b-a\|},\, y=\frac{b+a}{\|b+a\|}$$ and $$z=x\times y$$. Then $$x,y,z$$ form an orthonormal basis of $$\mathbb R^3$$, $$b=\sin\left(\frac{\phi}{2}\right)x+\cos\left(\frac{\phi}{2}\right)y$$ and the similar holds for $$a$$, except that the coefficient of $$x$$ is negated. Now let $$y'$$ and $$z'$$ be the results of rotating $$y$$ and $$z$$ about the axis $$x$$ by an angle $$\omega$$. That is, $$y'=\cos(\omega)y+\sin(\omega)z$$ $$z'=\cos(\omega)z-\sin(\omega)y.\tag{1}$$ Then $$b=\sin\left(\frac{\phi}{2}\right)x+\cos\left(\frac{\phi}{2}\right)\cos(\omega)y'-\cos\left(\frac{\phi}{2}\right)\sin(\omega)z'$$ and the similar holds for $$a$$ except that the coefficient of $$x$$ is negated. It follows that $$a$$ and $$b$$ have the same components along $$z'$$ and the angle $$\theta'$$ between their orthogonal projections on the $$xy'$$-plane is determined by the equation $$\tan\left(\frac{\theta'}{2}\right)=\frac{\tan(\phi/2)}{\cos(\omega)}$$. Hence $$\theta'=\theta$$ if we take $$\omega=\arccos\left(\frac{\tan(\phi/2)}{\tan(\theta/2)}\right).$$ With this $$\omega$$, we can pick a rotation matrix $$U$$ whose last column is the $$z'$$ in $$(1)$$.
When $$N\ge2$$ the problem can be reduced to a spherically constrained quadratic program that can be solved iteratively, but I will not go into details here.