/// <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];
}
}
}
}
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;
}
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;
}
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;
}
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;
}
