单片空间后方交会C#源代码

主方法：private void Cal_Click(object sender, EventArgs e){string[] lines = RichText.Text.Split('\n');long m = lines.Length;m = m - 1;//真实数据行数double[] Coor_x = new double[m];//已知点x坐标double[] Coor_y = new double[m];//已知点x坐标double[] Coor_X = new double[m];//已知点X坐标double[] Coor_Y = new double[m];//已知点Y坐标double[] Coor_Z = new double[m];//已知点Z坐标///赋值for (int i = 0; i < m; i++){string[] FJstring = Regex.Split(lines[i+1], ",");Coor_x[i] = 0.001*(Convert.ToDouble(FJstring[0]));Coor_y[i] = 0.001 *( Convert.ToDouble(FJstring[1]));Coor_X[i] = Convert.ToDouble(FJstring[2]);Coor_Y[i] = Convert.ToDouble(FJstring[3]);Coor_Z[i] = Convert.ToDouble(FJstring[4]);}if (textBox_m.Text == ""){MessageBox.Show("请输入参数！");}if (textBox_m.Text != ""){double M = double.Parse(textBox_m.Text);//比例尺double f = 0.001 * (double.Parse(textBox_f.Text));//焦距double x0 = 0.001 * double.Parse(textBox_x0.Text);//内方位元素x0double y0 = 0.001 * double.Parse(textBox_y0.Text);//内方位元素y0double X0 = 0, Y0 = 0, Z0 = 0;//外方位坐标元素初始值double min = (double.Parse(textBox_k.Text));//焦距double angle1 = 0, angle2 = 0, angle3 = 0;//外方位角元素初始值for (int i = 0; i < m; i++){//累加X0 = Coor_X[i] + X0;Y0 = Coor_Y[i] + Y0;Z0 = Coor_Z[i] + Z0;}X0 = X0 / m;Y0 = Y0 / m;Z0 = Z0 / m + M * f;double[,] A = new double[2 * m, 6];//A，二维：（2 * m）*6double[,] l = new double[2 * m, 1];//l，二维：（2 * m）*1double[,] V = new double[2 * m, 6];//V，二维：（2 * m）*6double[,] XXX = new double[6, 1];//XXX，6*1 所求参数值，即外方位元素改正数///循环解求误差方程，以限差为标准结束循环Matrix ad = new Matrix();Cls_AL ae = new Cls_AL();int num = 0;//记录循环次数for (int i = 0; ; i++){//求A，lae.Cal_AL(X0, Y0, Z0, angle1, angle2, angle3, m, f, x0, y0, Coor_x, Coor_y, Coor_X, Coor_Y, Coor_Z, A, l);//求XXXXXX = ad.Cal_Chengfa(ad.Cal_Chengfa(ad.Cal_Qiuni(ad.Cal_Chengfa(ad.Cal_Zhuanzhi(A), A)), ad.Cal_Zhuanzhi(A)), l);///改正外方位元素X0 = X0 + XXX[0, 0];Y0 = Y0 + XXX[1, 0];Z0 = Z0 + XXX[2, 0];angle1 = angle1 + XXX[3, 0];angle2 = angle2 + XXX[4, 0];angle3 = angle3 + XXX[5, 0];num++;if ((Math.Abs(XXX[3, 0]) < min) && (Math.Abs(XXX[4, 0]) < min) && (Math.Abs(XXX[5, 0]) < min)){ break; }}///精度评定;V = ad.Cal_Jianfa(ad.Cal_Chengfa(A, XXX), l);double[,] c = new double[1, 1];//单位权中误差c = ad.Cal_Chengfa(ad.Cal_Zhuanzhi(V), V);double cc = Math.Sqrt(c[0, 0] / (2 * m - 6));double[,] AA = new double[6, 6];//外方位元素改正数的协因数阵QQ AA = ad.Cal_Qiuni(ad.Cal_Chengfa(ad.Cal_Zhuanzhi(A), A));RichText.Text = ("外方位元素为：" + '\n' + "Xs = " + Convert.ToString(X0) + '\n' + "Ys = " + Convert.ToString(Y0) + '\n' + "Zs = " + Convert.ToString(Z0) + '\n' + "angel_1 = " + Convert.ToString(angle1) + '\n' + "angel_2 = " + Convert.ToString(angle2) + '\n' + "angel_3 = " + Convert.ToString(angle3) + '\n' + "单位权中误差为： " + Convert.ToString(cc) + '\n' + "Xs精度为： " + Convert.ToString(cc * Math.Sqrt(AA[0, 0])) + '\n' + "Ys精度为：" + Convert.ToString(cc * Math.Sqrt(AA[1, 1])) + '\n' + "Zs精度为：" + Convert.ToString(cc * Math.Sqrt(AA[2, 2])) + '\n' + "迭代次数为：" + Convert.ToString(num));}}动态类库：using System;using System.Collections.Generic;using System.Linq;using System.Text;namespace dll{/// <summary>/// Matrix类包含三个方法：矩阵乘法，矩阵转置，矩阵求逆/// </summary>public class Matrix{/// <summary>/// 矩阵乘法/// </summary>/// <param name="a"></param>/// <param name="b"></param>/// <returns></returns>public double[,] Cal_Chengfa(double[,] a, double[,] b){//定义一个以a行数为行数，b列数为列数的数组double[,] c = new double[a.GetLength(0), b.GetLength(1)];for (int i = 0; i < c.GetLength(0); i++){for (int j = 0; j < c.GetLength(1); j++)c[i, j] = 0;for (int k = 0; k < a.GetLength(1); k++){c[i, j] += a[i, k] * b[k, j]; //i行j列元素结果}}}return c;}/// <summary>/// 矩阵减法/// </summary>/// <param name="a"></param>/// <param name="b"></param>/// <returns></returns>public double[,] Cal_Jianfa(double[,] a, double[,] b){//定义一个以a行数为行数，b列数为列数的数组int m = a.GetLength(0);int n = a.GetLength(1);double[,] c = new double[m, m];for (int i = 0; i < m; i++){for (int j = 0; j < n; j++){c[i, j] = a[i, j] - b[i, j];}}return c;}/// <summary>/// 矩阵转置/// </summary>/// <param name="a"></param>/// <returns></returns>public double[,] Cal_Zhuanzhi(double[,] a){double[,] b = new double[a.GetLength(1), a.GetLength(0)];for (int i = 0; i < a.GetLength(1); i++){for (int j = 0; j < a.GetLength(0); j++)b[i, j] = a[j, i];}}return b;}/// <summary>/// 矩阵求逆/// </summary>/// <param name="a"></param>/// <returns></returns>public double[,] Cal_Qiuni(double[,] a){double[,] b = new double[a.GetLength(0), a.GetLength(1)]; int i, j, row, k;double max, temp;//单位矩阵for (i = 0; i < a.GetLength(1); i++){b[i, i] = 1;}for (k = 0; k < a.GetLength(1); k++){max = 0; row = k;//找最大元，其所在行为rowfor (i = k; i < a.GetLength(1); i++){temp = Math.Abs(a[i, k]);if (max < temp){max = temp;row = i;}}//交换k与row行if (row != k){for (j = 0; j < a.GetLength(0); j++){temp = a[row, j];a[row, j] = a[k, j];a[k, j] = temp;temp = b[row, j];b[row, j] = b[k, j];b[k, j] = temp;}}//首元化为1for (j = k + 1; j < a.GetLength(0); j++) a[k, j] /= a[k, k];for (j = 0; j < a.GetLength(0); j++) b[k, j] /= a[k, k];a[k, k] = 1;//k列化为0//对afor (j = k + 1; j < a.GetLength(0); j++){for (i = 0; i < k; i++) a[i, j] -= a[i, k] * a[k, j];for (i = k + 1; i < a.GetLength(0); i++) a[i, j] -= a[i, k] * a[k, j];}//对bfor (j = 0; j < a.GetLength(0); j++){for (i = 0; i < k; i++) b[i, j] -= a[i, k] * b[k, j];for (i = k + 1; i < a.GetLength(0); i++) b[i, j] -= a[i, k] * b[k, j];}for (i = 0; i < a.GetLength(0); i++) a[i, k] = 0;a[k, k] = 1;}return b;}}public class Cls_AL{//计算A，L阵public void Cal_AL(double X0, double Y0, double Z0, double angle1, double angle2, double angle3, long m, double f, double x0, double y0, double[] Coor_x, double[] Coor_y, double[] Coor_X, double[] Coor_Y, double[] Coor_Z, double[,] A, double[,] l){//旋转矩阵double a1 = Math.Cos(angle1) * Math.Cos(angle3) - Math.Sin(angle1) * Math.Sin(angle2) * Math.Sin(angle3);double a2 = -Math.Cos(angle1) * Math.Sin(angle3) - Math.Sin(angle1) * Math.Sin(angle2) * Math.Cos(angle3);double a3 = -Math.Sin(angle1) * Math.Cos(angle2);double b1 = Math.Cos(angle2) * Math.Sin(angle3);double b2 = Math.Cos(angle2) * Math.Cos(angle3);double b3 = -Math.Sin(angle2);double c1 = Math.Sin(angle1) * Math.Cos(angle3) + Math.Cos(angle1) * Math.Sin(angle2) * Math.Sin(angle3);double c2 = -Math.Sin(angle1) * Math.Sin(angle3) + Math.Cos(angle1) * Math.Sin(angle2) * Math.Cos(angle3);double c3 = Math.Cos(angle1) * Math.Cos(angle2);///系数double[] a11 = new double[m];double[] a12 = new double[m];double[] a13 = new double[m];double[] a21 = new double[m];double[] a22 = new double[m];double[] a23 = new double[m];double[] a14 = new double[m];double[] a15 = new double[m];double[] a16 = new double[m];double[] a24 = new double[m];double[] a25 = new double[m];double[] a26 = new double[m];for (int i = 0; i < m; i++){double Z = ((a3 * (Coor_X[i] - X0)) + (b3 * (Coor_Y[i] - Y0)) + (c3 * (Coor_Z[i] - Z0)));//a11[i] = ((a1 * f) + (a3 * Coor_x[i])) / Z;a12[i] = ((b1 * f) + (b3 * Coor_x[i])) / Z;a13[i] = ((c1 * f) + (c3 * Coor_x[i])) / Z;a21[i] = ((a2 * f) + (a3 * Coor_y[i])) / Z;a22[i] = ((b2 * f) + (b3 * Coor_y[i])) / Z;a23[i] = ((c2 * f) + (c3 * Coor_y[i])) / Z;a14[i] = Coor_y[i] * Math.Sin(angle2) - Math.Cos(angle2)*((Coor_x[i] / f) * (Coor_x[i] * Math.Cos(angle3) - Coor_y[i]*Math .Sin (angle3))+f*Math .Cos(angle3));a15[i] = -f * Math.Sin(angle3) - Coor_x[i] * (Coor_x[i] * Math.Sin(angle3) + Coor_y[i] * Math.Cos(angle3)) / f;a16[i] = Coor_y[i];a24[i] = -Coor_x[i] * Math.Sin(angle2) - Coor_y[i] * Math.Cos(angle2) * (Coor_x[i] * Math.Cos(angle3) - Coor_y[i] * Math.Sin(angle3) - f * Math.Sin(angle3)) / f;a25[i] = -f * Math.Cos(angle3) - Coor_y[i] * (Coor_x[i] * Math.Sin(angle3) + Coor_y[i] * Math.Cos(angle3)) / f;a26[i] = -Coor_x[i];A[2 * i, 0] = a11[i];A[2 * i, 1] = a12[i];A[2 * i, 2] = a13[i];A[2 * i, 3] = a14[i];A[2 * i, 4] = a15[i];A[2 * i, 5] = a16[i];A[2 * i + 1, 0] = a21[i];A[2 * i + 1, 1] = a22[i];A[2 * i + 1, 2] = a23[i];A[2 * i + 1, 3] = a24[i];A[2 * i + 1, 4] = a25[i];A[2 * i + 1, 5] = a26[i];l[2 * i, 0] = -x0+Coor_x[i] + f * ((a1 * (Coor_X[i] - X0)) + (b1 * (Coor_Y[i] - Y0)) + (c1 * (Coor_Z[i] - Z0))) / Z;l[2 * i + 1, 0] = -y0+Coor_y[i] + f * ((a2 * (Coor_X[i] - X0)) + (b2 * (Coor_Y[i] - Y0)) + (c2 * (Coor_Z[i] - Z0))) / ((a3 * (Coor_X[i] - X0)) + (b3 * (Coor_Y[i] - Y0)) + (c3 * (Coor_Z[i] - Z0)));}}}}测试数据：x,y,z,X,Y,Z-86.15,-68.99,36589.41,25273.32,2195.17-53.40,82.21,37631.08,31324.51,728.69-14.78,-76.63,39100.97,24934.98,2386.5010.46,64.43,40426.54,30319.81,757.31。