/// <summary>
/// 获取观测方程。 -DeltaX - ((1+k)*RotateMatrix*X - Y)， 或者 Y = DeltaX + (1+k)*RotateMatrix*X (布尔沙方程)
/// </summary>
/// <param name="X">原始点</param>
/// <param name="Y">目标点</param>
/// <returns></returns>
private double[,] GetF(double[,] X, double[,] Y)
{
double[,] f0;
double[,] qx = MatrixTool.Init(X.GetLength(0), 1);
double[,] K = { { -dx }, { -dy } };
//double[,] S = { { 1 } };
//MatrixTool.Multi(X, S, ref qx);
double[,] M = GetM();
qx = specialMulti(M, X);
MatrixTool.Sub(qx, Y, ref qx);
f0 = specialSub(K, qx);
return(f0);
}
private double[,] specialSub(double[,] m, double[,] X)
{
int rowNumM = m.GetLength(0);
int colNumM = m.GetLength(1);
int rowNumX = X.GetLength(0);
int colNumX = X.GetLength(1);
int lines = rowNumX / rowNumM;
double[,] subX = MatrixTool.Init(rowNumM, colNumX);
double[,] res = MatrixTool.Init(rowNumX, colNumX);
for (int i = 0; i < rowNumX; i += rowNumM)
{
MatrixTool.CopySub(X, i, 0, rowNumM, colNumX, ref subX, 0, 0);
MatrixTool.Sub(m, subX, ref subX);
MatrixTool.CopySub(subX, 0, 0, rowNumM, colNumX, ref res, i, 0);
}
return(res);
}