//计算旋转矩阵
internal static double[,] GetRotationMatrix(double[,] RotationAngles)
{
//RotationAngles[3, 1] = [φ; ω; к]
double phi = RotationAngles[0, 0];
double omega = RotationAngles[1, 0];
double kappa = RotationAngles[2, 0];
double[,] Rphi = { { Math.Cos(phi), 0, -Math.Sin(phi) }, { 0, 1, 0 }, { Math.Sin(phi), 0, Math.Cos(phi) } };
double[,] Romega = { { 1, 0, 0 }, { 0, Math.Cos(omega), -Math.Sin(omega) }, { 0, Math.Sin(omega), Math.Cos(omega) } };
double[,] Rkappa = { { Math.Cos(kappa), -Math.Sin(kappa), 0 }, { Math.Sin(kappa), Math.Cos(kappa), 0 }, { 0, 0, 1 } };
return(MatrixComputation.Multiply(MatrixComputation.Multiply(Rphi, Romega), Rkappa));
}
//求Q
internal static double[,] GetQ(double[,] A, double[,] X, double[,] L, int PointCount)
{
/*
* 平差课本
* (P130，7-3-2) 单位权中误差σ0 = √((V^(T)*PV)/r)
* P = I
* (P112，7-1-4)V = AX - L
* r = n - t
* (P132，表7-9) Qxx = NBB^(-1) = (A^(T)*PA)^(-1)
* (P133，7-3-15) σx = σ0 * √Qxx
*/
double m0 = Math.Sqrt((MatrixComputation.Multiply(MatrixComputation.Transpose(MatrixComputation.Subtract(MatrixComputation.Multiply(A, X), L)), MatrixComputation.Subtract(MatrixComputation.Multiply(A, X), L))[0, 0] / (2 * PointCount - 6)));
double[,] Q = MatrixComputation.Inverse(MatrixComputation.Multiply(MatrixComputation.Transpose(A), A));
for (int i = 0; i < Q.GetLength(0); i++)
{
for (int j = 0; j < Q.GetLength(1); j++)
{
Q[i, j] = Math.Sqrt(Q[i, j]) * m0;
}
}
return(Q);
}
public static Matrix operator *(Matrix A, Matrix B)
{
return(MatrixComputation.Multiply(A.elements, B.elements));
}
//求改正数
internal static double[,] GetX(double[,] A, double[,] L)
{
//X = (A^(T)A)^(-1)*A^(T)*L
return(MatrixComputation.Multiply(MatrixComputation.Multiply(MatrixComputation.Inverse((MatrixComputation.Multiply(MatrixComputation.Transpose(A), A))), MatrixComputation.Transpose(A)), L));
}