Example #1
0
        /// <summary>
        /// From first to second
        /// </summary>
        /// <param name="M1"></param>
        /// <param name="M2"></param>
        public static void CopyMatrix(Matrix2x2 M1, Matrix2x2 M2)
        {
            for (int jcx = 0; jcx < Matrix2x2.dim; jcx++)
                {
                    for (int jcy = 0; jcy < Matrix2x2.dim; jcy++)
                    {

                        for (int jcr = 0; jcr < dim; jcr++)
                        {
                            M2[jcx, jcy] = M1[jcx, jcr];
                        }
                    }
                }
        }
Example #2
0
 public Matrix2x2 Transpone()
 {
     Matrix2x2 mt2x2 = new Matrix2x2();
     for (int jcx = 0; jcx < dim; jcx++)
         for (int jcy = 0; jcy < dim; jcy++)
             mt2x2[jcx, jcy] = M2x2[jcy, jcx];
     return mt2x2;
 }
Example #3
0
 public Matrix2x2 Inverse()
 {
     double det = M2x2[0,0]*M2x2[1,1] - M2x2[0,1]*M2x2[1,0];
     if(det==0)
             return null;
     double det1 = 1/det;
     Matrix2x2 m2x2 = new Matrix2x2();
     m2x2[0,0] = M2x2[1,1]*det1;
     m2x2[0,1] = -(M2x2[0,1]*det1);
     m2x2[1, 0] = -(M2x2[1, 0] * det1);
     m2x2[1, 1] = M2x2[0, 0] * det1;
     return m2x2;
 }
Example #4
0
 public static Matrix2x2 Sum(Matrix2x2 m12x2, Matrix2x2 m22x2)
 {
     Matrix2x2 mm2x2 = new Matrix2x2();
     for (int jcx = 0; jcx < dim; jcx++)
         for (int jcy = 0; jcy < dim; jcy++)
             mm2x2[jcx, jcy] = m12x2[jcx, jcy] + m22x2[jcx, jcy];
     return mm2x2;
 }
Example #5
0
 public static Matrix2x2 Multiply(Matrix2x2 m12x2, Matrix2x2 m22x2)
 {
     Matrix2x2 mm2x2 = new Matrix2x2();
     for (int jcx = 0; jcx < dim; jcx++)
     {
         for (int jcy = 0; jcy < Matrix2x2.dim; jcy++)
         {
             mm2x2[jcx, jcy] = 0;
             for (int jcr = 0; jcr < dim; jcr++)
             {
                 mm2x2[jcx, jcy] += m12x2[jcx, jcr] * m22x2[jcr, jcy];
             }
         }
     }
     return mm2x2;
 }
 /// <summary>
 ///  расстояние[0] ... угол[1]
 /// </summary>
 /// <param name="vec"></param>
 /// <returns></returns>
 public double[] CalcEstimation(double[] vec)
 {
     xEsimate = MatrixTrans.MulVector(xEsimateSave);
     // F*p*F' + Q
     Matrix2x2 mTrans1 = Matrix2x2.Multiply(MatrixTrans, MatrixError);
     Matrix2x2 mTrans2 = MatrixTrans.Transpone();
     Matrix2x2 mTrans3 = Matrix2x2.Multiply(mTrans1, mTrans2);
     MatrixError = Matrix2x2.Sum(mTrans3, MatrixNoiseProcess);
     ////Correction K = P*H'(H*P*H' + R)^-1
     mTrans2 = MatrixMeasure.Transpone(); // H'
     mTrans1 = Matrix2x2.Multiply(MatrixError, mTrans2); //P*H'
     mTrans3 = Matrix2x2.Multiply(MatrixMeasure, mTrans1);  //H*P*H'
     mTrans3 = Matrix2x2.Sum(mTrans3,MatrixIdent); //H*P*H' + R
     mTrans3 = mTrans3.Inverse();
     if (mTrans3 == null)
         return vec;
     Matrix2x2 mCorr = Matrix2x2.Multiply(mTrans1, mTrans3);//K
     ///x = x +K*(z-H*x);
     double[] transVector = MatrixMeasure.MulVector(xEsimate); //H*x
     transVector = Matrix2x2.SubVectors(vec, transVector);
     transVector = mCorr.MulVector(transVector);//K*(z-H*x)
     xEsimateSave = Matrix2x2.SumVectors(transVector, xEsimate);
     //P = (I -K * H)*P;
     mTrans1 = Matrix2x2.Multiply(mCorr, MatrixMeasure); //K * H
     mTrans2 = Matrix2x2.Sub(MatrixIdent, mTrans1);//I -K * H
     MatrixError = Matrix2x2.Multiply(mTrans2, MatrixError);
     return xEsimate;
 }