Ejemplo n.º 1
0
        /// <summary>
        /// Kalman滤波计算不变参数。
        /// </summary>
        /// <param name="input"></param>
        /// <returns></returns>
        private AdjustResultMatrix GetSequentialConst(AdjustObsMatrix input)
        {
            WeightedVector appri = null;
            Matrix         B2    = input.BuildCoeefOfConstParam();

            if (LastConstResult != null)
            {
                var IsEqual = Geo.Utils.ListUtil.IsEqual(LastConstResult.ParamNames, input.SecondParamNames);
                if (IsEqual)
                {
                    appri = LastConstResult.Estimated;
                }
                else
                {
                    appri = SimpleAdjustMatrixBuilder.GetNewWeighedVectorInOrder(input.SecondParamNames, LastConstResult.Estimated);
                }
            }
            if (appri == null)//第一次,使用参数平差结果
            {
                ParamAdjuster paramAdjuster = new ParamAdjuster();
                var           paramResult   = paramAdjuster.Run(new AdjustObsMatrix(input.Observation, B2, null, input.SecondParamNames));
                appri = paramResult.Estimated;
            }

            AdjustObsMatrix obsMatrix1 = new AdjustObsMatrix(appri, input.Observation, B2, input.SecondTransfer);

            obsMatrix1.ParamNames = input.SecondParamNames;

            //var kalmanFilter = new SimpleKalmanFilter();

            var res = MatrixAdjuster.Run(obsMatrix1);

            return(res);
        }
Ejemplo n.º 2
0
        /// <summary>
        /// 计算
        /// </summary>
        /// <param name="input"></param>
        /// <returns></returns>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            //try
            //{
            this.ObsMatrix = input;
            int paramCount = input.Coefficient.ColCount;

            Predict(input.Transfer, InverseWeightOfTransfer);

            var est1 = CorrectSimple(input.Observation - input.FreeVector, input.Coefficient, input.Observation.InverseWeight);
            // var est2 = CorrectNormal(this.Observation, CoeffOfParam, Observation.InverseWeight);
            // var est3 = NewCorrect(this.Observation, CoeffOfParam, Observation.InverseWeight);

            //var differ12 = est1 - est2;
            //var differ23 = est2 - est3;
            //var differ13 = est1 - est3;

            var Estimated             = est1;
            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetEstimated(Estimated)
                                        .SetObsMatrix(input)
                                        .SetFreedom(Freedom)
                                        .SetVarianceFactor(VarianceOfUnitWeight)
                                        .SetVtpv(vtpv)
            ;

            return(result);
            //}
            //catch (Exception ex)
            //{
            //    log.Error("SimpleKalmanFilter 滤波出错了", ex);
            //}
        }
Ejemplo n.º 3
0
        public static AdjustObsMatrix BuildATtest(int paramCount, int obsCount, AdjustmentType adjustmentType = AdjustmentType.参数平差)
        {
            bool isParamOrCondition = adjustmentType == AdjustmentType.参数平差;
            int  restCount          = obsCount - paramCount;
            int  rowOfCoeef         = isParamOrCondition ? obsCount : restCount;
            int  colOfCoeef         = isParamOrCondition ? paramCount : obsCount;

            AdjustObsMatrix obsMatrix = new AdjustObsMatrix();

            obsMatrix.Observation = WeightedVector.GenerateATest(obsCount);
            obsMatrix.Apriori     = WeightedVector.GenerateATest(paramCount);
            obsMatrix.Coefficient = BuildACoeefient(rowOfCoeef, colOfCoeef);
            obsMatrix.Transfer    = new WeightedMatrix(Matrix.CreateIdentity(paramCount), Matrix.CreateIdentity(paramCount));

            obsMatrix.FreeVector         = new Vector(rowOfCoeef, 1);
            obsMatrix.SecondFreeVector   = new Vector(paramCount, 1);
            obsMatrix.ApproxVector       = new Vector(paramCount, 1);
            obsMatrix.SecondApproxVector = new Vector(paramCount, 1);

            if (adjustmentType == AdjustmentType.具有参数的条件平差)
            {
                obsMatrix.SecondCoefficient = BuildACoeefient(rowOfCoeef, colOfCoeef);
            }


            return(obsMatrix);
        }
Ejemplo n.º 4
0
        /// <summary>
        /// 实时计算,数据计算
        /// </summary>
        /// <param name="input">观测矩阵</param>
        /// <returns></returns>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            if (input == this.ObsMatrix)
            {
                return(this.LastResult);
            }
            this.ObsMatrix = input;

            switch (StepOfRecursive)
            {
            case StepOfRecursive.SuperposOfConstNeq:
                return(GetConstParamResult(input, NormalEquationSuperposer));

            //case StepOfRecursive.ComputeMutableParam:  break;
            case StepOfRecursive.SequentialConst:
                AdjustResultMatrix res = GetSequentialConst(input);
                this.LastConstResult = res;
                return(res);

            case StepOfRecursive.RealTime:    //实时计算,参数变化可能带来错误
                AdjustResultMatrix result = GetRealTimeResult(input);
                this.LastResult = result;
                return(result);

            case StepOfRecursive.ParamAdjust:              //参数逐历元平差
                return(GetSimpleParamAdjustResult(input)); //参数平差验证,2018.10.15, czs, hmx, 验证第一个结果是一样的。

            default:
                break;
            }
            return(null);
        }
Ejemplo n.º 5
0
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            var result = ConditionalAdjuster.Run(input);



            throw new NotImplementedException();
        }
Ejemplo n.º 6
0
        private AdjustResultMatrix GetRealTimeResult(AdjustObsMatrix input)
        {
            //构建不变参数的法方程
            var newConstParamNe = input.BuildConstParamNormalEquation();

            NormalEquationSuperposer.Add(newConstParamNe);                 //添加到法方程迭加器中
            WeightedVector     estY   = NormalEquationSuperposer.GetEstimated();
            AdjustResultMatrix result = Step3GetMutableX(estY, ObsMatrix); //求异变参数

            return(result);
        }
Ejemplo n.º 7
0
        /// <summary>
        /// 运行
        /// </summary>
        /// <param name="input"></param>
        /// <returns></returns>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            //原始输入
            Matrix B  = new Matrix(input.Coefficient);
            Matrix L  = new Matrix((IMatrix)input.Observation);
            Matrix QL = new Matrix((IMatrix)input.Observation.InverseWeight);
            Matrix B0 = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;//B0

            Matrix PL      = QL.Inversion;
            int    freedom = B.RowCount;
            Matrix BT      = B.Trans;

            int obsCount   = L.RowCount;
            int paramCount = 0;


            Matrix         W      = -(B * L - B0);
            Matrix         N      = B * QL * BT;
            Matrix         inverN = N.Inversion;
            Matrix         K      = inverN * W;
            Matrix         Vhat   = (QL * BT * K);
            Matrix         Qvhat  = QL * BT * inverN * B * QL;
            WeightedVector estLW  = new WeightedVector(Vhat, Qvhat)
            {
                ParamNames = input.Observation.ParamNames
            };

            Matrix         Lhat         = L + Vhat;
            Matrix         QhatL        = QL - Qvhat;
            WeightedVector correctedObs = new WeightedVector(Lhat, QhatL)
            {
                ParamNames = input.Observation.ParamNames
            };

            double vtpv = (Vhat.Trans * PL * Vhat).FirstValue;
            double s0   = vtpv / freedom;//单位权中误差估值

            if (!DoubleUtil.IsValid(s0))
            {
                log.Error("方差值无效!" + s0);
            }

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetAdjustmentType(AdjustmentType.条件平差)
                                        .SetEstimated(estLW)
                                        .SetCorrectedObs(correctedObs)
                                        .SetObsMatrix(input)
                                        .SetFreedom(freedom)
                                        .SetVarianceFactor(s0)
                                        .SetVtpv(vtpv);

            return(result);
        }
        /// <summary>
        /// 计算
        /// </summary>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            //命名规则:0表示上一个,1表示预测,无数字表示当次
            //上次次观测设置
            var X0  = new Matrix((IMatrix)input.Apriori);
            var Qx0 = new Matrix(input.Apriori.InverseWeight);
            var Px0 = new Matrix(Qx0.GetInverse());
            //本次观测设置
            var Qo = new Matrix(input.Observation.InverseWeight);
            var Po = new Matrix(Qo.GetInverse());
            var A  = new Matrix(input.Coefficient);
            var AT = A.Trans;
            var L  = new Matrix((IMatrix)input.Observation);

            int paramCount = A.ColCount;
            int obsCount   = A.RowCount;

            //1.预测残差
            //计算预测残差
            var V1  = L - A * X0;//观测值 - 估计近似值
            var Qv1 = Qo + A * Qx0 * AT;



            //2.计算增益矩阵
            var J = Qx0 * AT * Qv1.Inversion;// 增益矩阵
            //3.平差结果
            var dX = J * V1;
            var X  = X0 + dX;

            //4.精度评定
            var Qx      = Qx0 - J * A * Qx0;
            var Freedom = input.Observation.Count - input.ParamCount + input.Apriori.Count;

            var V    = A * dX - V1;                    //估值-观测值 V = A * X - L = A * (X0 + deltaX) - (l + A * X0) =  A * deltaX - l.
            var vtpv = (V.Trans * Po * V)[0, 0];
            var VarianceOfUnitWeight = vtpv / Freedom; //单位权方差
            var Estimated            = new WeightedVector(X, Qx)
            {
                ParamNames = input.ParamNames
            };


            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetEstimated(Estimated)
                                        .SetFreedom(Freedom)
                                        .SetObsMatrix(input)
                                        .SetVarianceFactor(VarianceOfUnitWeight)
                                        .SetVtpv(vtpv)
            ;

            return(result);
        }
Ejemplo n.º 9
0
        /// <summary>
        /// 恢复参数平差。
        /// </summary>
        /// <param name="input"></param>
        /// <returns></returns>
        private static AdjustResultMatrix GetSimpleParamAdjustResult(AdjustObsMatrix input)
        {
            #region 合并系数阵
            Matrix A = new Matrix(input.Coefficient.RowCount, input.Coefficient.ColCount + input.SecondCoefficient.ColCount);
            A.SetSub(input.Coefficient);
            A.SetSub(input.SecondCoefficient, 0, input.Coefficient.ColCount);
            var names = new List <string>();
            names.AddRange(input.ParamNames);
            names.AddRange(input.SecondParamNames);
            #endregion

            ParamAdjuster   paramAdjuster = new ParamAdjuster();
            AdjustObsMatrix adjustObs     = new AdjustObsMatrix(input.Observation, A, null, names);
            var             resultMatrix  = paramAdjuster.Run(adjustObs);
            return(resultMatrix);
        }
Ejemplo n.º 10
0
        /// <summary>
        /// 解析
        /// </summary>
        /// <param name="text"></param>
        /// <param name="splitterOfMatrixItems">分隔矩阵元素的分隔符</param>
        /// <returns></returns>
        public static AdjustObsMatrix Parse(string text, string[] splitterOfMatrixItems = null)
        {
            AdjustObsMatrix obs = new AdjustObsMatrix();

            if (splitterOfMatrixItems == null)
            {
                splitterOfMatrixItems = new string[] { ",", ";", "\t", " " };
            }

            string[] lines = text.Split(new char[] { '\r', '\n' }, StringSplitOptions.None); //保留空格以分隔

            //解析本对象属性
            ParseHeader(obs, lines);

            int len       = lines.Length;
            var typeNames = Enum.GetNames(typeof(ObsMatrixType));

            StringBuilder currentBlockSb = new StringBuilder();
            ObsMatrixType currentType    = ObsMatrixType.Apriori;
            ObsMatrixType nextType       = ObsMatrixType.Apriori;

            for (int i = 0; i < len; i++)
            {
                string line = lines[i].Trim();                                    //当前行
                if (typeNames.Contains(line))                                     //新区
                {
                    currentType = nextType;                                       //
                    nextType    = Geo.Utils.EnumUtil.Parse <ObsMatrixType>(line); //这是解析的下一个区的名称

                    if (currentBlockSb.Length > 2)
                    {
                        ParseBufferText(splitterOfMatrixItems, obs, currentBlockSb, currentType);
                    }
                    continue;
                }

                currentBlockSb.AppendLine(line);
            }

            if (currentBlockSb.Length > 2)
            {
                currentType = nextType;//
                //解析最后一个
                ParseBufferText(splitterOfMatrixItems, obs, currentBlockSb, currentType);
            }
            return(obs);
        }
Ejemplo n.º 11
0
        /// <summary>
        /// 解析为对象
        /// </summary>
        /// <param name="splitterOfMatrixItems"></param>
        /// <param name="obs"></param>
        /// <param name="currentBlockSb"></param>
        /// <param name="currentType"></param>
        private static void ParseBufferText(string[] splitterOfMatrixItems, AdjustObsMatrix obs, StringBuilder currentBlockSb, ObsMatrixType currentType)
        {
            switch (currentType)
            {
            case ObsMatrixType.Apriori:
                obs.Apriori = WeightedVector.Parse(currentBlockSb.ToString());
                break;

            case ObsMatrixType.Observation:
                obs.Observation = WeightedVector.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
                break;

            case ObsMatrixType.Coefficient:
                obs.Coefficient = Matrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
                break;

            case ObsMatrixType.SecondCoefficient:
                obs.SecondCoefficient = Matrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
                break;

            case ObsMatrixType.Transfer:
                obs.Transfer = WeightedMatrix.Parse(currentBlockSb.ToString(), splitterOfMatrixItems);
                break;

            case ObsMatrixType.ApproxVector:
                obs.ApproxVector = Vector.Parse(currentBlockSb.ToString());
                break;

            case ObsMatrixType.SecondApproxVector:
                obs.SecondApproxVector = Vector.Parse(currentBlockSb.ToString());
                break;

            case ObsMatrixType.FreeVector:
                obs.FreeVector = Vector.Parse(currentBlockSb.ToString());
                break;

            case ObsMatrixType.SecondFreeVector:
                obs.SecondFreeVector = Vector.Parse(currentBlockSb.ToString());
                break;

            default:
                break;
            }
            currentBlockSb.Clear();
        }
Ejemplo n.º 12
0
        /// <summary>
        /// 条件平差法解算固定解,将固定解当成虚拟观测量,对原浮点解进行约束,条件平差。
        /// </summary>
        /// <param name="coeffOfParam">系数阵,条件方程构造</param>
        /// <param name="totalFloat">原浮点解</param>
        /// <param name="fixedObs">已经固定的参数,固定解当成虚拟观测量</param>
        /// <returns></returns>
        public static WeightedVector SolveAmbiFixedResultByConditionAdjust(WeightedVector totalFloat, Vector fixedObs, IMatrix coeffOfParam)
        {
            //以下算法已经验证等价!!2018.09.02, czs, hmx
            bool           isSong       = false;
            WeightedVector NewEstimated = null;

            if (isSong)
            {
                #region 求固定解 宋力杰方法
                IMatrix X_old         = totalFloat;
                IMatrix QX_old        = totalFloat.InverseWeight;
                IMatrix coeffOfParamT = coeffOfParam.Transposition;

                IMatrix W = coeffOfParam.Multiply(X_old).Minus(new VectorMatrix(fixedObs));

                IMatrix tmp = coeffOfParam.Multiply(QX_old).Multiply(coeffOfParamT);

                IMatrix Nadd = (QX_old.Multiply(coeffOfParamT)).Multiply(tmp.GetInverse());

                IMatrix X_new = X_old.Minus(Nadd.Multiply(W));

                IMatrix tmp2   = Nadd.Multiply(coeffOfParam);
                IMatrix QX_new = QX_old.Minus(tmp2.Multiply(QX_old));

                NewEstimated = new WeightedVector(X_new, QX_new)
                {
                    ParamNames = coeffOfParam.ColNames
                };
                #endregion
            }
            else
            {
                //条件平差
                AdjustObsMatrix obsMatrix = new AdjustObsMatrix();
                obsMatrix.SetCoefficient(coeffOfParam).SetObservation(totalFloat).SetFreeVector(fixedObs);
                ConditionalAdjuster adjuster = new ConditionalAdjuster();
                var resultMatrix             = adjuster.Run(obsMatrix);

                NewEstimated = resultMatrix.CorrectedObs;
            }

            return(NewEstimated);
        }
Ejemplo n.º 13
0
        /// <summary>
        /// 批量总体技术。参数不要改变,否则达不到预期效果。
        /// </summary>
        /// <param name="inputs"></param>
        /// <returns></returns>
        public List <AdjustResultMatrix> Run(List <AdjustObsMatrix> inputs)
        {
            AdjustObsMatrix firstMatrix = inputs[0];
            Matrix          Y0all       = firstMatrix.HasSecondApprox ? new Matrix(firstMatrix.SecondApproxVector, true) : null;
            WeightedVector  estY        = GetConstY(inputs);

            //Matrix Y = Y0all + constY;
            //step 3:求易变参数
            List <AdjustResultMatrix> results = new List <AdjustResultMatrix>();

            foreach (var obsMatrix in inputs)
            {
                this.ObsMatrix = obsMatrix;

                AdjustResultMatrix result = Step3GetMutableX(estY, obsMatrix);

                results.Add(result);
            }
            return(results);
        }
Ejemplo n.º 14
0
        /// <summary>
        /// 解析本对象属性
        /// </summary>
        /// <param name="obs"></param>
        /// <param name="lines"></param>
        private static void ParseHeader(AdjustObsMatrix obs, string[] lines)
        {
            var headerSpliter     = new char[] { ':', ':' };
            var headerItemSpliter = new char[] { ',', ' ', '\t', ';' };

            foreach (var line in lines)
            {
                if (String.IsNullOrEmpty(line))
                {
                    continue;
                }
                if (line.Contains("×"))
                {
                    break;
                }

                var items = line.Split(headerSpliter, StringSplitOptions.RemoveEmptyEntries);
                if (items.Length == 1)
                {
                    continue;
                }

                var header  = items[0];
                var content = items[1];
                switch (header)
                {
                case "ParamNames":
                    obs.ParamNames = new List <string>(content.Split(headerItemSpliter, StringSplitOptions.RemoveEmptyEntries));
                    break;

                case "SecondParamNames":
                    obs.SecondParamNames = new List <string>(content.Split(headerItemSpliter, StringSplitOptions.RemoveEmptyEntries));
                    break;

                default: break;
                }
            }
        }
Ejemplo n.º 15
0
        /// <summary>
        /// 数据计算
        /// </summary>
        /// <param name="input">观测矩阵</param>
        /// <returns></returns>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            this.ObsMatrix = input;

            //观测值权阵设置,对已知量赋值
            Matrix L          = new Matrix((IMatrix)input.Observation);
            Matrix QL         = new Matrix(input.Observation.InverseWeight);
            Matrix PL         = new Matrix(QL.GetInverse());
            Matrix A          = new Matrix(input.Coefficient);
            Matrix AT         = A.Trans;
            Matrix X0         = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;
            Matrix D          = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;
            Matrix dN         = input.CoeffIncrementOfNormalEquation;
            int    obsCount   = A.RowCount;
            int    paramCount = A.ColCount;
            int    freedom    = obsCount - paramCount;
            //观测值更新
            Matrix l = L - (A * X0 + D); //如果null,则是本身

            //法方程
            Matrix N = new Matrix(SymmetricMatrix.Parse(AT * PL * A + dN));
            Matrix U = AT * PL * l;

            Matrix InverN = N.Inversion;

            //平差结果
            Matrix x  = InverN * U;
            Matrix Qx = InverN - dN;
            //精度评定
            Matrix V  = A * x - l;
            Matrix Qv = QL - A * Qx * AT;
            Matrix X  = X0 + x;

            double vtpv = (V.Trans * PL * V).FirstValue;
            double s0   = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差

            WeightedVector estX = new WeightedVector(x, Qx)
            {
                ParamNames = input.ParamNames
            };
            WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
            {
                ParamNames = input.ParamNames
            };
            WeightedVector estV = new WeightedVector(V, Qv)
            {
                ParamNames = this.ObsMatrix.Observation.ParamNames
            };

            Matrix Lhat         = L + V;
            Matrix QLhat        = A * Qx * AT;
            var    correctedObs = new WeightedVector(Lhat, QLhat)
            {
                ParamNames = this.ObsMatrix.Observation.ParamNames
            };


            if (!DoubleUtil.IsValid(s0))
            {
                log.Error("方差值无效!" + s0);
            }

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetAdjustmentType(AdjustmentType.参数平差)
                                        .SetEstimated(estX)
                                        .SetCorrection(estV)
                                        .SetCorrectedObs(correctedObs)
                                        .SetCorrectedEstimate(CorrectedEstimate)
                                        .SetObsMatrix(input)
                                        .SetFreedom(freedom)
                                        .SetVarianceFactor(s0)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 16
0
        /// <summary>
        /// 参数加权平差
        /// </summary>
        /// <param name="input"></param>
        /// <returns></returns>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            //下标 o 表示观测值,x表示估计值,xa0表示具有先验信息的随机参数,
            //xa为包含先验信息xa0的矩阵,是在整个误差方程中计算的矩阵
            var Freedom = input.ObsCount - (input.ParamCount - input.Apriori.RowCount); // n -tb

            Matrix A          = new Matrix(input.Coefficient);
            Matrix AT         = A.Trans;
            Matrix L          = new Matrix((IMatrix)input.Observation);
            Matrix Qo         = new Matrix(input.Observation.InverseWeight);
            Matrix Po         = Qo.Inversion;
            int    obsCount   = L.RowCount;
            int    paramCount = A.ColCount;

            //具有先验信息的随机参数
            Matrix Xa0  = new Matrix((IMatrix)input.Apriori);
            Matrix Qxa0 = new Matrix(input.Apriori.InverseWeight);
            Matrix Pxa0 = Qxa0.Inversion;

            //计算先验信息的平差矩阵部分
            Matrix Nxa0 = new Matrix(input.ParamCount);

            Nxa0.SetSub(Pxa0);

            Matrix Uxa0 = Pxa0 * Xa0;
            Matrix Uxa  = new Matrix(input.ParamCount, 1);

            Uxa.SetSub(Uxa0);

            //法方程系数阵
            Matrix N      = AT * Po * A + Nxa0;
            Matrix InverN = N.Inversion;

            //法方程右手边
            Matrix U = AT * Po * L + Uxa;

            //计算估值
            Matrix X = InverN * U;

            //精度估计
            Matrix Xa  = X.GetSub(0, 0, Xa0.RowCount);
            Matrix dXa = Xa - Xa0;

            Matrix V    = A * X - L;
            Matrix VT   = V.Trans;
            var    vtpv = (VT * Po * V + dXa.Trans * Pxa0 * dXa).FirstValue;
            var    VarianceOfUnitWeight = vtpv / Freedom;

            var Estimated = new WeightedVector(X, InverN)
            {
                ParamNames = input.ParamNames
            };
            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetEstimated(Estimated)
                                        .SetObsMatrix(input)
                                        .SetFreedom(Freedom)
                                        .SetVarianceFactor(VarianceOfUnitWeight)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 17
0
        /// <summary>
        /// 数据处理。全部转化为计算偏移量,即,各种参数采用近似值,此处需要考虑初值情况。
        /// </summary>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            //参数命名规则:下标 0 表示上一个,1 表示预测,无数字表示当次
            #region 参数预测
            WeightedVector apriori = input.Apriori;
            if (apriori == null)
            {
                throw new ArgumentException("必须具有先验参数值。");
            }
            //先验值赋值
            ArrayMatrix X0  = new ArrayMatrix(apriori);                                //上一次参数估值
            ArrayMatrix Qx0 = new ArrayMatrix(apriori.InverseWeight.Array);            //上一次估计误差方差权逆阵

            ArrayMatrix Trans  = new ArrayMatrix(input.Transfer.Array);                //状态转移矩阵
            ArrayMatrix TransT = Trans.Transpose();
            ArrayMatrix Q_m    = new ArrayMatrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声

            //计算参数预测值,可以看做序贯平差中的第一组数据
            ArrayMatrix X1  = Trans * X0;
            ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;

            var Predicted = new WeightedVector(X1, Qx1)
            {
                ParamNames = input.ParamNames
            };                                                                            //结果为残差
            #endregion

            //System.IO.File.WriteAllText(saveDir + @"\Predicted.txt", Predicted.ToFormatedText());
            //  System.IO.File.WriteAllText(saveDir + @"\Apriori.txt", Apriori.ToFormatedText());

            #region 参数估计
            ArrayMatrix A  = new ArrayMatrix(input.Coefficient.Array); //误差方程系数阵
            ArrayMatrix AT = A.Transpose();                            //A 的转置

            //估计值才需要观测值,而预测值不需要
            //观测值赋值
            WeightedVector obs = input.Observation - input.FreeVector;
            if (obs == null)
            {
                throw new ArgumentException("必须具有观测向量。");
            }

            ArrayMatrix L   = new ArrayMatrix(obs);                     //观测值,或 观测值 - 估计值,!!
            ArrayMatrix Q_o = new ArrayMatrix(obs.InverseWeight.Array); //观测噪声权逆阵
            ArrayMatrix P_o = Q_o.Inverse;


            //计算预测的观测残差 自由项
            //由 V = A X - L, 得 V = A x - l, l = L - A X0, X = X0 + x
            ArrayMatrix dL  = L - A * X1;//此处注意符号
            ArrayMatrix QdL = Q_o + A * Qx1 * AT;
            //计算平差值的权阵
            ArrayMatrix PXk = AT * P_o * A + Qx1.Inverse;
            //计算平差值的权逆阵
            ArrayMatrix Qx = PXk.Inverse;
            //计算增益矩阵
            ArrayMatrix J = Qx * AT * P_o;
            //计算参数改正值和估值
            ArrayMatrix deltaX = J * dL;
            ArrayMatrix X      = X1 + deltaX;//改 X0 为 X1

            //精度估计
            ArrayMatrix UnitMatrix = ArrayMatrix.EyeMatrix(J.RowCount, 1.0);
            ArrayMatrix B          = UnitMatrix - J * A;
            Qx = B * Qx1 * B.Transposition + J * Q_o * J.Transposition; //参数权逆阵

            //   Matrix Qx = (AT * P_o * A + Qx1.Inverse).Inverse;

            var Estimated = new WeightedVector(X, Qx)
            {
                ParamNames = input.ParamNames
            };
            #endregion

            #region 验后观测残差
            // Matrix Px = Qx.Inverse;
            ArrayMatrix V = L - A * X;

            this.PostfitObservation = new Vector(MatrixUtil.GetColVector(V.Array))
            {
                ParamNames = input.ParamNames
            };
            #endregion

            #region 精度估计
            this.SumOfObsCount += input.ObsCount;
            var Freedom = SumOfObsCount - input.ParamCount;// input.Freedom;

            //观测噪声权阵
            ArrayMatrix V1TPV1 = deltaX.Transpose() * Qx1.Inverse * deltaX;
            ArrayMatrix VTPV   = V.Transpose() * P_o * V;
            var         vtpv   = VTPV[0, 0];
            double      upper  = (V1TPV1 + VTPV)[0, 0];


            if (!DoubleUtil.IsValid(upper) || upper > 1e10 || upper < 0)
            {
                log.Debug("方差值无效!" + upper);
            }

            //赋值
            var VarianceOfUnitWeight = Math.Abs(upper) / Freedom;

            //System.IO.File.WriteAllText(saveDir + @"\Estimated.txt", Estimated.ToFormatedText());
            //System.IO.File.WriteAllText(saveDir + @"\Observation.txt", Observation.ToFormatedText());

            #endregion

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetEstimated(Estimated)
                                        .SetFreedom(Freedom)
                                        .SetObsMatrix(input)
                                        .SetVarianceFactor(VarianceOfUnitWeight)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 18
0
        /// <summary>
        /// 计算
        /// </summary>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            this.ObsMatrix = input;

            //命名规则:0表示上一个(先验信息),1表示预测,无数字表示当次
            //上次次观测设置
            var Qx0 = new Matrix(input.Apriori.InverseWeight);
            var Px0 = new Matrix(Qx0.GetInverse());
            var X0  = new Matrix((IMatrix)input.Apriori);

            //本次观测设置
            var Qo         = new Matrix(input.Observation.InverseWeight);
            var Po         = new Matrix(Qo.GetInverse());
            var A          = new Matrix(input.Coefficient);
            var AT         = A.Trans;
            var L          = new Matrix((IMatrix)input.Observation);
            int obsCount   = L.RowCount;
            int paramCount = A.ColCount;

            //具有状态转移的序贯平差
            //if (input.Transfer != null && input.InverseWeightOfTransfer!=null)
            //{
            //    Matrix Trans = new Matrix(input.Transfer.Array);           //状态转移矩阵
            //    Matrix TransT = Trans.Transpose();
            //    Matrix Q_m = new Matrix(input.InverseWeightOfTransfer.Array);    //状态转移模型噪声

            //    //计算参数预测值,可以看做序贯平差中的第一组数据
            //    //ArrayMatrix X1 = Trans * X0;
            //    //ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
            //    X0 = Trans * X0;
            //    Qx0 = Trans * Qx0 * TransT + Q_m;
            //    Px0 = Qx0.Inversion;
            //}



            //1.预测残差
            //计算预测残差
            var V1  = L - A * X0;             //观测值 - 估计近似值
            var Qv1 = Qo + A * Qx0 * AT;      //预测残差方差
            //2.计算增益矩阵
            var J = Qx0 * AT * Qv1.Inversion; // 增益矩阵
            //3.平差结果
            var dX = J * V1;                  //参数改正
            var X  = X0 + dX;

            //4.精度评定
            var Qx        = Qx0 - J * A * Qx0;
            var Estimated = new WeightedVector(X, Qx)
            {
                ParamNames = input.ParamNames
            };

            SumOfObsCount += input.ObsCount;
            var Freedom = SumOfObsCount - input.ParamCount;// AprioriObsCount;
            //  var V = A * dX - V1;//估值-观测值 V = A * X - L = A * (X0 + deltaX) - (l + A * X0) =  A * deltaX - l.
            //  this.VarianceOfUnitWeight = (V.Trans * Po * V).FirstValue / Freedom;//单位权方差

            Matrix V    = A * X - L;
            Matrix Vx   = X - X0;
            Matrix VTPV = null;

            if (Po.IsDiagonal)
            {
                VTPV = new Matrix(AdjustmentUtil.ATPA(V, Po)) + (Vx.Trans * Px0 * Vx);
            }
            else
            {
                VTPV = V.Trans * Po * V + (Vx.Trans * Px0 * Vx);
            }

            var vtpv = VTPV[0, 0];

            this.SumOfVptv     += vtpv;
            this.SumOfObsCount += A.RowCount;
            var VarianceOfUnitWeight = Math.Abs(vtpv / (Freedom));


            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetEstimated(Estimated)
                                        .SetFreedom(Freedom)
                                        .SetObsMatrix(input)
                                        .SetVarianceFactor(VarianceOfUnitWeight)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 19
0
        /// <summary>
        /// 构建不变参数的计算结果。
        /// 注意:需要控制参数的增减问题。
        /// 基本思路:增加则插入,减少则删除,通过参数名称来控制。
        /// </summary>
        /// <param name="obsMatrix"></param>
        /// <param name="NormalEquationSuperposer"></param>
        /// <returns></returns>
        public static AdjustResultMatrix GetConstParamResult(AdjustObsMatrix obsMatrix, NormalEquationSuperposer NormalEquationSuperposer)
        {
            //观测值权阵设置,对已知量赋值
            Matrix L               = new Matrix((IMatrix)obsMatrix.Observation);
            Matrix QL              = new Matrix(obsMatrix.Observation.InverseWeight);
            Matrix PL              = new Matrix(QL.GetInverse());
            Matrix A               = new Matrix(obsMatrix.Coefficient);
            Matrix AT              = A.Trans;
            Matrix B               = new Matrix(obsMatrix.SecondCoefficient);
            Matrix BT              = B.Trans;
            Matrix X0              = obsMatrix.HasApprox ? new Matrix(obsMatrix.ApproxVector, true) : null;
            Matrix Y0              = obsMatrix.HasSecondApprox ? new Matrix(obsMatrix.SecondApproxVector, true) : null;
            Matrix D               = obsMatrix.HasFreeVector ? new Matrix(obsMatrix.FreeVector, true) : null;
            int    obsCount        = L.RowCount;
            int    fixedParamCount = obsMatrix.SecondParamNames.Count;// B.ColCount;
            int    freedom         = obsCount - fixedParamCount;

            //观测值更新
            Matrix lxy = L - (A * X0 + B * Y0 + D); //采用估值计算的观测值小量

            Matrix ATPL = AT * PL;
            //法方程
            Matrix Na      = ATPL * A;
            Matrix Nab     = AT * PL * B;
            Matrix InverNa = Na.Inversion;
            Matrix J       = A * InverNa * AT * PL;
            Matrix I       = Matrix.CreateIdentity(J.RowCount);
            Matrix B2      = (I - J) * B; //新的系数阵 Ac, 原文中为 B波浪~
            Matrix B2T     = B2.Trans;
            Matrix B2TPL   = B2T * PL;
            Matrix NofB2   = B2TPL * B2;
            Matrix UofB2   = B2TPL * lxy;

            NofB2.ColNames = obsMatrix.SecondParamNames;
            NofB2.RowNames = obsMatrix.SecondParamNames;

            UofB2.RowNames = obsMatrix.SecondParamNames;
            UofB2.ColNames = new List <string>()
            {
                "ConstParam"
            };

            //生成法方程
            var ne = new MatrixEquation(NofB2, UofB2);

            //叠加法方程
            NormalEquationSuperposer.Add(ne);//添加到法方程迭加器中

            var acNe = NormalEquationSuperposer.CurrentAccumulated;

            Matrix inverN = acNe.N.Inversion;
            Matrix y      = inverN * acNe.U;

            y.RowNames = acNe.ParamNames;
            Matrix Qy = inverN;

            Qy.ColNames = acNe.ParamNames;
            Qy.RowNames = acNe.ParamNames;
            var estY = new WeightedVector(y, Qy)
            {
                ParamNames = acNe.ParamNames
            };

            var    V    = B2 * y - lxy;
            Matrix Qv   = QL - B2 * Qy * B2T;
            Matrix Y    = Y0 + y;
            var    vtpv = (V.Trans * PL * V).FirstValue;
            double s0   = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差

            WeightedVector CorrectedEstimate = new WeightedVector(Y, Qy);

            WeightedVector estV = new WeightedVector(V, Qv)
            {
                ParamNames = obsMatrix.Observation.ParamNames
            };
            Matrix Lhat         = L + V;
            Matrix QLhat        = B2 * Qy * B2T;
            var    correctedObs = new WeightedVector(Lhat, QLhat)
            {
                ParamNames = obsMatrix.Observation.ParamNames
            };

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetAdjustmentType(AdjustmentType.递归最小二乘)
                                        .SetEstimated(estY)
                                        .SetCorrection(estV)
                                        .SetCorrectedObs(correctedObs)
                                        .SetCorrectedEstimate(CorrectedEstimate)
                                        .SetObsMatrix(obsMatrix)
                                        .SetFreedom(freedom)
                                        .SetVarianceFactor(s0)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 20
0
        /// <summary>
        /// 第三步:计算易变参数
        /// </summary>
        /// <param name="estY"></param>
        /// <param name="obsMatrix"></param>
        /// <returns></returns>
        private AdjustResultMatrix Step3GetMutableX(WeightedVector estY, AdjustObsMatrix obsMatrix)
        {
            Matrix constY  = estY.GetVectorMatrix();
            Matrix constQy = estY.InverseWeight;

            //观测值权阵设置,对已知量赋值
            Matrix L                 = new Matrix((IMatrix)obsMatrix.Observation);
            Matrix QL                = new Matrix(obsMatrix.Observation.InverseWeight);
            Matrix PL                = new Matrix(QL.GetInverse());
            Matrix A                 = new Matrix(obsMatrix.Coefficient);
            Matrix AT                = A.Trans;
            Matrix B                 = new Matrix(obsMatrix.SecondCoefficient);
            Matrix BT                = B.Trans;
            Matrix X0                = obsMatrix.HasApprox ? new Matrix(obsMatrix.ApproxVector, true) : null;
            Matrix Y0                = obsMatrix.HasSecondApprox ? new Matrix(obsMatrix.SecondApproxVector, true) : null;
            Matrix D                 = obsMatrix.HasFreeVector ? new Matrix(obsMatrix.FreeVector, true) : null;
            int    obsCount          = L.RowCount;
            int    fixedParamCount   = B.ColCount;
            int    mutableParamCount = A.ColCount;
            int    paramCount        = fixedParamCount + mutableParamCount;
            int    freedom           = obsCount - paramCount;

            Matrix lxy  = L - (A * X0 + B * Y0 + D); //采用估值计算的观测值小量
            Matrix ATPL = AT * PL;
            //法方程
            Matrix Na      = ATPL * A;
            Matrix Nab     = AT * PL * B;
            Matrix InverNa = Na.Inversion;

            //求x
            //观测值更新,采用估值进行计算
            Matrix lx = lxy - B * constY;                     // = L - (A * X0 + B * Y + D);
                                                              /*  Matrix x = InverNa * ATPL * lx;*/
            Matrix x = InverNa * (ATPL * lxy - Nab * constY); //这两个计算是等价的
            Matrix X = X0 + x;

            Matrix Ntmp = Na.Inversion * Nab;
            Matrix Qx   = InverNa + Ntmp * constQy * Ntmp.Trans;

            //Matrix Qxy = AT * PL * B;
            //Matrix Qtemp = Qx * Qxy;
            //Matrix Dx = Qx + Qtemp * Dy * Qtemp.Trans;

            //精度评定
            Matrix V  = A * x - lx;
            Matrix Qv = QL - A * Qx * AT - B * constQy * BT;

            // Matrix lT = l.Trans;

            double vtpv = (V.Trans * PL * V).FirstValue;       //(lT * PL * l - lT * PL * Ac * y).FirstValue;//

            double s0 = vtpv / (freedom == 0 ? 0.1 : freedom); //单位权方差

            WeightedVector estX = new WeightedVector(x, Qx)
            {
                ParamNames = obsMatrix.ParamNames
            };
            WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
            {
                ParamNames = obsMatrix.ParamNames
            };
            WeightedVector estV = new WeightedVector(V, Qv)
            {
                ParamNames = obsMatrix.Observation.ParamNames
            };

            Matrix Lhat         = L + V;
            Matrix QLhat        = A * Qx * AT;
            var    correctedObs = new WeightedVector(Lhat, QLhat)
            {
                ParamNames = this.ObsMatrix.Observation.ParamNames
            };


            if (!DoubleUtil.IsValid(s0))
            {
                log.Error("方差值无效!" + s0);
            }

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetAdjustmentType(AdjustmentType.递归最小二乘)
                                        .SetEstimated(estX)
                                        .SetSecondEstimated(estY)
                                        .SetCorrection(estV)
                                        .SetCorrectedObs(correctedObs)
                                        .SetCorrectedEstimate(CorrectedEstimate)
                                        .SetObsMatrix(obsMatrix)
                                        .SetFreedom(freedom)
                                        .SetVarianceFactor(s0)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 21
0
        /// <summary>
        /// 数据计算
        /// </summary>
        /// <param name="input">观测矩阵</param>
        /// <returns></returns>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            this.ObsMatrix = input;

            //观测值权阵设置,对已知量赋值
            Matrix L               = new Matrix((IMatrix)input.Observation);
            Matrix QL              = new Matrix(input.Observation.InverseWeight);
            Matrix PL              = new Matrix(QL.GetInverse());
            Matrix A               = new Matrix(input.Coefficient);
            Matrix AT              = A.Trans;
            Matrix B               = new Matrix(input.SecondCoefficient);
            Matrix BT              = B.Trans;
            Matrix X0              = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;
            Matrix Y0              = input.HasSecondApprox ? new Matrix(input.SecondApproxVector, true) : null;
            Matrix D               = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;
            int    obsCount        = L.RowCount;
            int    fixedParamCount = B.ColCount;

            //观测值更新
            Matrix l = L - (A * X0 + B * Y0 + D); //如果null,则是本身

            Matrix ATPL = AT * PL;
            //法方程
            Matrix Na      = ATPL * A;
            Matrix Nab     = AT * PL * B;
            Matrix InverNa = Na.Inversion;
            Matrix J       = A * InverNa * AT * PL;
            Matrix B2      = (Matrix.CreateIdentity(J.RowCount) - J) * B;
            Matrix AcT     = B2.Trans;
            Matrix Nc      = AcT * PL * B2;;
            Matrix InverNc = Nc.Inversion;
            //平差结果
            Matrix y  = InverNc * AcT * PL * l;
            Matrix Y  = Y0 + y;
            Matrix Qy = InverNc;

            //只针对y的精度评定
            Matrix W   = B2 * y - l;
            double ys0 = (W.Trans * PL * W).FirstValue / (obsCount - fixedParamCount);
            Matrix Dy  = ys0 * Qy;


            //求x
            Matrix lx = L - (A * X0 + B * Y + D); //如果null,则是本身
            Matrix x  = InverNa * ATPL * lx;      //这两个计算是等价的
            // Matrix x = InverNa * (ATPL * l - Nab * y);//这两个计算是等价的
            Matrix X = X0 + x;

            Matrix Ntmp = Na.Inversion * Nab;
            Matrix Qx   = InverNa + Ntmp * Qy * Ntmp.Trans;

            //Matrix Qxy = AT * PL * B;
            //Matrix Qtemp = Qx * Qxy;
            //Matrix QtempT = Qtemp.Trans;
            //Matrix Dx = Qx + Qtemp * Dy * QtempT;

            //精度评定
            Matrix V  = A * x + B * y - l;
            Matrix Qv = QL - A * Qx * AT - B * Qy * BT;

            // Matrix lT = l.Trans;

            double vtpv       = (V.Trans * PL * V)[0, 0];//(lT * PL * l - lT * PL * Ac * y).FirstValue;//
            int    paramCount = A.ColCount + B.ColCount;
            int    freedom    = A.RowCount - paramCount;
            double s0         = vtpv / (freedom == 0 ? 0.1 : freedom);//单位权方差

            WeightedVector estX = new WeightedVector(x, Qx)
            {
                ParamNames = input.ParamNames
            };
            WeightedVector CorrectedEstimate = new WeightedVector(X, Qx)
            {
                ParamNames = input.ParamNames
            };
            WeightedVector estY = new WeightedVector(y, Qy)
            {
                ParamNames = input.SecondParamNames
            };
            WeightedVector estV = new WeightedVector(V, Qv);

            Matrix Lhat         = L + V;
            Matrix QLhat        = A * Qx * AT;
            var    correctedObs = new WeightedVector(Lhat, QLhat)
            {
                ParamNames = this.ObsMatrix.Observation.ParamNames
            };


            if (!DoubleUtil.IsValid(s0))
            {
                log.Error("方差值无效!" + s0);
            }

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetAdjustmentType(AdjustmentType.单期递归最小二乘)
                                        .SetEstimated(estX)
                                        .SetSecondEstimated(estY)
                                        .SetCorrection(estV)
                                        .SetCorrectedObs(correctedObs)
                                        .SetCorrectedEstimate(CorrectedEstimate)
                                        .SetObsMatrix(input)
                                        .SetFreedom(freedom)
                                        .SetVarianceFactor(s0)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 22
0
        /// <summary>
        /// 计算
        /// </summary>
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            this.ObsMatrix = input;

            //命名规则:0表示上一个(先验信息),1表示预测,无数字表示当次
            //上次次观测设置
            var Qx0 = new Matrix(input.Apriori.InverseWeight);
            var Px0 = new Matrix(Qx0.GetInverse());
            var X0  = new Matrix((IMatrix)input.Apriori);

            //本次观测设置
            var Qo         = new Matrix(input.Observation.InverseWeight);
            var Po         = new Matrix(Qo.GetInverse());
            var A          = new Matrix(input.Coefficient);
            var AT         = A.Trans;
            var L          = new Matrix((IMatrix)input.Observation);
            int obsCount   = L.RowCount;
            int paramCount = A.ColCount;

            //具有状态转移的序贯平差
            if (input.Transfer != null && input.InverseWeightOfTransfer != null)
            {
                Matrix Trans  = new Matrix(input.Transfer.Array);                //状态转移矩阵
                Matrix TransT = Trans.Transpose();
                Matrix Q_m    = new Matrix(input.InverseWeightOfTransfer.Array); //状态转移模型噪声

                //计算参数预测值,可以看做序贯平差中的第一组数据
                //ArrayMatrix X1 = Trans * X0;
                //ArrayMatrix Qx1 = Trans * Qx0 * TransT + Q_m;
                //更新先验值
                X0  = Trans * X0;
                Qx0 = Trans * Qx0 * TransT + Q_m;
                Px0 = Qx0.Inversion;
            }
            var ATP = AT * Po;
            var N   = ATP * A;
            var U   = ATP * L;
            var Px  = (N + Px0);
            var Qx  = Px.Inversion;
            var X   = Qx * (U + Px0 * X0);


            var Estimated = new WeightedVector(X, Qx)
            {
                ParamNames = input.ParamNames
            };

            SumOfObsCount += input.ObsCount;
            var Freedom = SumOfObsCount - input.ParamCount;// AprioriObsCount;

            Matrix V    = A * X - L;
            Matrix Vx   = X - X0;
            Matrix VTPV = null;

            if (Po.IsDiagonal)
            {
                VTPV = new Matrix(AdjustmentUtil.ATPA(V, Po)) + (Vx.Trans * Px0 * Vx);
            }
            else
            {
                VTPV = V.Trans * Po * V + (Vx.Trans * Px0 * Vx);
            }

            var vtpv = VTPV[0, 0];

            this.SumOfObsCount += A.RowCount;
            var VarianceOfUnitWeight = Math.Abs(vtpv / (Freedom));

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetEstimated(Estimated)
                                        .SetFreedom(Freedom)
                                        .SetObsMatrix(input)
                                        .SetVarianceFactor(VarianceOfUnitWeight)
                                        .SetVtpv(vtpv);

            return(result);
        }
Ejemplo n.º 23
0
        public override AdjustResultMatrix Run(AdjustObsMatrix input)
        {
            //u(0<u<t)个独立量为参数,多余观测数r=n-t
            //原始输入 m=r+u
            Matrix A  = input.Coefficient;                                                           //m×t
            Matrix B  = input.SecondCoefficient;                                                     //m×n
            Matrix L  = new Matrix((IMatrix)input.Observation);                                      //n×1
            Matrix QL = new Matrix((IMatrix)input.Observation.InverseWeight);                        //m×m
            Matrix X0 = input.HasApprox ? new Matrix(input.ApproxVector, true) : null;               //u×1
            Matrix D  = input.HasFreeVector ? new Matrix(input.FreeVector, true) : null;             //B0//m×1
            Matrix B0 = input.HasSecondFreeVector ? new Matrix(input.SecondFreeVector, true) : null; //B0//m×1

            Matrix AT = A.Trans;
            Matrix PL = QL.Inversion;
            Matrix BT = B.Trans;

            //多余观测数为r=m-u=n-t
            int obsCount            = L.RowCount;
            int totalConditionCount = B.RowCount;
            int paramCount          = A.ColCount;//u
            int freedom             = obsCount - paramCount;

            //观测值更新
            Matrix l      = L - (A * X0 + D); //如果null,则是本身
            Matrix W      = -(B * X0 - B0);
            Matrix N      = AT * PL * A;
            Matrix U      = AT * PL * l;
            Matrix inverN = N.Inversion;
            // ************************
            //采用分块矩阵直接计算
            int    dimOfX    = N.ColCount; //X的维度
            int    dimOfK    = B.RowCount; //K的维度
            int    dimOfBigN = dimOfK + dimOfX;
            Matrix BigN      = new Matrix(dimOfBigN, dimOfBigN);

            BigN.SetSub(N);
            BigN.SetSub(BT, 0, dimOfX);
            BigN.SetSub(B, dimOfX, 0);

            Matrix inverBigN = BigN.Inversion;
            Matrix bigW      = new Matrix(dimOfBigN, 1);

            bigW.SetSub(U);
            bigW.SetSub(W, U.RowCount);
            Matrix bigX = inverBigN * bigW;

            Matrix x = bigX.GetSub(0, 0, dimOfX);
            Matrix K = bigX.GetSub(dimOfX, 0);

            Matrix X = X0 + x;

            Matrix V    = A * x - l;
            Matrix EstL = L + V;

            //**************精度估计---------------
            Matrix Q11 = inverBigN.GetSub(0, 0, dimOfX, dimOfX);
            Matrix Q12 = inverBigN.GetSub(0, dimOfX, dimOfK);
            Matrix Q21 = inverBigN.GetSub(dimOfX, 0, dimOfK, dimOfK);
            Matrix Q22 = inverBigN.GetSub(dimOfX, dimOfX);

            Matrix Qx    = Q11;
            Matrix QestL = A * Q11 * AT;

            var coWeightedX = new WeightedVector(X, Qx);
            var weightedX   = new WeightedVector(x, Qx);
            var weightedL   = new WeightedVector(EstL, QestL);


            double vtpv = (V.Trans * PL * V).FirstValue;
            double s0   = vtpv / freedom;//单位权中误差估值

            if (!DoubleUtil.IsValid(s0))
            {
                log.Error("方差值无效!" + s0);
            }

            AdjustResultMatrix result = new AdjustResultMatrix()
                                        .SetAdjustmentType(AdjustmentType.具有参数的条件平差)
                                        .SetEstimated(weightedX)
                                        .SetCorrectedEstimate(coWeightedX)
                                        .SetCorrectedObs(weightedL)
                                        .SetObsMatrix(input)
                                        .SetFreedom(freedom)
                                        .SetVarianceFactor(s0)
                                        .SetVtpv(vtpv);

            return(result);
        }