间接平差程序实现

using CMath;
using System;
using System.Collections.Generic;
using System.Text;

namespace Adjust
{
    /// <summary>
    /// 线性方程，即Bx+V=0
    /// </summary>
    public class LinearEquation
    {
        public Matrix B
        {
            get;
            set;
        }
        private Matrix V;
        public Matrix P
        {
            get;
            set;
        }
        public Matrix l
        {
            get;
            set;
        }
        /// <summary>
        /// 获取x的维数
        /// </summary>
        public int N
        {
            get
            {
                return V.Rows;
            }
        }
        /// <summary>
        /// 用系数矩阵B和V初始化线性方程，B和V的行数必须相等
        /// </summary>
        /// <param name="B">系数矩阵</param>
        /// <param name="V">残差改正值</param>
        /// <param name="P">观测权，若为空，则为单位阵</param>
        public LinearEquation(Matrix B,Matrix l,Matrix P=null)
        {
            if (B.Rows != l.Rows)
            {
                throw new Exception("B和V的行数不相等");
            }
            else
            {
                this.B = B;
                this.l = l;
            }
            if (P == null)
            {
                double[,] i=new double[B.Rows,B.Rows];
                for (int j = 0; j < i.GetLength(0); j++)
                {
                    for (int k = 0; k < i.GetLength(1); k++)
                    {
                        i[j, k] = 0;
                        if (j == k)
                        {
                            i[j, k] = 1;
                        }
                    }
                }
                //P为单位矩阵E
                this.P = new Matrix(i);
            }
        }
        /// <summary>
        /// 得到法方程矩阵系数
        /// </summary>
        /// <returns>得到法方程矩阵系数</returns>
        public Matrix GetBTPB()
        {
            return this.B.Transpose() * this.P * this.B;
        }
        /// <summary>
        /// 得到法方程参数
        /// </summary>
        /// <returns>得到法方程参数</returns>
        public Matrix GetBTPL()
        {
            return this.B.Transpose() * this.P * this.l;
        }
        /// <summary>
        /// 得到所选参数的改正值
        /// </summary>
        /// <returns>得到所选参数的改正值</returns>
        public Matrix GetNW()
        {
            return this.GetBTPB().Inverse() * this.GetBTPL();
        }
        public Matrix GetV()
        {
            return this.B * this.GetNW() - this.l;
        }
        /// <summary>
        /// 得到单位权中误差
        /// </summary>
        /// <returns>单位权中误差</returns>
        public double GetSigam()
        {
            Matrix Sigma = this.GetV().Transpose() * this.P * this.GetV();
            return Math.Sqrt(Sigma[0, 0] / (this.B.Rows - this.B.Cols));
        }
        public override string ToString()
        {
            this.V = this.GetV();
            StringBuilder sb = new StringBuilder();
            for (int i = 0; i < this.N; i++)
            {
                for (int j = 0; j < B.Cols; j++)
                    sb.Append(B[i, j] + " ");
                sb.Append(" －　"+l[i,0]+" = ");
                for(int j=0;j<V.Cols;j++)
                {
                    //是否要加空格
                    string flag="";
                    if (j != V.Cols - 1) flag = "  ";
                    sb.Append(V[i, j] + flag);
                }
                //判断是否换行
                string lineflag ="";
                if (i != this.N - 1) lineflag = Environment.NewLine;
                sb.Append(lineflag);
            }
            return sb.ToString();
        }
    }
}