单像空间后方交会计算偏差方程系数矩阵

double XBar, YBar, ZBar;   //将共线条件方程简化
 double[] GeoX = null, GeoY = null, GeoZ = null;   //地面点坐标
 double[,] R = new double[3, 3];                         //声明旋转矩阵
 double a1, a2, a3, b1, b2, b3, c1, c2, c3;            //旋转元素
 double Xs, Ys, Zs, Phi, Omega, Kappa;             //定义外方位元素
 double[,] A;

A = new double[2 * No, 6];
                for (int i = 0; i < 2 * No; i++)
                { 
 
  
                    XBar = a1 * (GeoX[k] - Xs) + b1 * (GeoY[k] - Ys) + c1 * (GeoZ[k] - Zs);
                    YBar = a2 * (GeoX[k] - Xs) + b2 * (GeoY[k] - Ys) + c2 * (GeoZ[k] - Zs);
                    ZBar = a3 * (GeoX[k] - Xs) + b3 * (GeoY[k] - Ys) + c3 * (GeoZ[k] - Zs);
                    if (i % 2 == 0)//数组第奇数行a11,a31等
                    { 
 
  
                        A[i, 0] = (a1 * f + a3 * Phox[k]) / ZBar;//a11
                        A[i, 1] = (b1 * f + b3 * Phox[k]) / ZBar;//a12
                        A[i, 2] = (c1 * f + c3 * Phox[k]) / ZBar;//a13
                        A[i, 3] = Phoy[k] * Math.Sin(Omega) - (Phox[k] * (Phox[k] * Math.Cos(Kappa) - Phoy[k] * Math.Sin(Kappa)) / f + f * Math.Cos(Kappa)) * Math.Cos(Omega);//a14
                        A[i, 4] = -f * Math.Sin(Kappa) - Phox[k] * (Phox[k] * Math.Sin(Kappa) + Phoy[k] * Math.Cos(Kappa)) / f;//a15
                        A[i, 5] = Phoy[k]; //a16
                    }
                    else  //偶数行
                    { 
 
  
                        A[i, 0] = (a2 * f + a3 * Phoy[k]) / ZBar;//a21
                        A[i, 1] = (b2 * f + b3 * Phoy[k]) / ZBar;//a22
                        A[i, 2] = (c2 * f + c3 * Phoy[k]) / ZBar;//a23
                        A[i, 3] = -Phox[k] * Math.Sin(Omega) - (Phoy[k] * (Phox[k] * Math.Cos(Kappa) - Phoy[k] * Math.Sin(Kappa) / f) - f * Math.Sin(Kappa)) * Math.Cos(Omega);//a24
                        A[i, 4] = -f * Math.Cos(Kappa) - (Phoy[k] * (Phoy[k] - Math.Sin(Kappa) + Phoy[k] * Math.Cos(Kappa))) / f;//a25
                        A[i, 5] = -Phox[k];//a26
                        k++;
                    }
                }