迭代非线性最小二乘优化3d到2d投影

请原谅我的英语不好，这不是我的母语。

我试图解决一个非线性最小二乘问题。我有两组点。时间t的一组3d点和时间t+1的一组2d图像点，知道它们之间的对应关系。

现在我有了下面的等式:

x(t+1) = ( x(t) - y(t)*rotz + z(t)*roty + tx )*f/(-x(t)*roty + y(t)*rotx+z(t)+tz)
y(t+1) = ( x(t)*rotz + y(t) - z(t)*rotx + ty )*f/(-x(t)*roty + y(t)*rotx+z(t)+tz)

变量f为焦距。我想确定rotx, roty, rotz, tx, ty, tz，使计算点(t+1的图像点)与已知图像点(t+1)之间的残差最小。

取图像点之间的欧几里得距离作为误差测量。

E = sqrt( (x(t+1) - X)^2 + (y(t+1) - Y)^2 )

X和Y为已知的2d图像点。

我计算了一个矩阵a，每个点对应一行，列中有rox, roty等的偏导数。

我还用E的结果计算了列向量y，用于对rotx, roty等的第一次估计。

现在我可以计算变量的更新:

delta = inv(A' * A) * A' * y

(A'表示A的转置)在更新之后，我使用这些变量进行更多的迭代。

但算法不收敛，即使我设置第一个估计与期望解。所以我觉得我做错了什么。

我是否使用了错误的方法?谁能给我一个链接到一个很好的例子，或者可以解释他的方法来解决这个问题。

非常感谢!: -)