double[] Update(double[] vec)
{
////Correction K = P*H'(H*P*H' + R)^-1
//mTrans2 = MatrixMeasure.Transpone(); // H'
double[] mTrans3 = Matrix2x2.MulVector2(vError, vMeasure); //P*H'
double[] mTrans4 = Matrix2x2.MulVector2(vMeasure, mTrans3); //H*P*H'
double[] mTrans5 = Matrix2x2.SumVectors(mTrans4, vE); //H*P*H' + R
double[] mTrans6 = Matrix2x2.InverseVec(mTrans5);
if (mTrans6 == null)
{
return(vec);
}
double[] mTrans7 = Matrix2x2.MulVector2(vTrans, vError);
double[] mCorr = Matrix2x2.MulVector2(mTrans7, mTrans6); //K
///x = x +K*(z-H*x);
double[] transVector1 = Matrix2x2.MulVector2(vMeasure, xEsimateV); //H*x
double[] transVector2 = Matrix2x2.SubVectors(vec, transVector1);
double[] transVector3 = Matrix2x2.MulVector2(mCorr, transVector2); //K*(z-H*x)
xEsimateSaveV = Matrix2x2.SumVectors(transVector3, xEsimateV);
//P = (I -K * H)*P;
double[] mTrans8 = Matrix2x2.MulVector2(mCorr, vMeasure); //K * H
double[] mTrans9 = Matrix2x2.SubVectors(vE, mTrans8); //I -K * H
vError = Matrix2x2.MulVector2(mTrans9, vError);
/*
* FormStringForLog("vec ", vec[0], vec[1]);
* FormStringForLog("xEsimateV ", xEsimateV[0], xEsimateV[1]);
* FormStringForLog("xEsimateSaveV ", xEsimateSaveV[0], xEsimateSaveV[1]);
* FormStringForLog("Del xEsimateV ", Math.Abs(xEsimateV[0] - vec[0]), Math.Abs(xEsimateV[1] - vec[1]));
* FormStringForLog("Del xEsimateSaveV ", Math.Abs(xEsimateSaveV[0] - vec[0]), Math.Abs(xEsimateSaveV[0] - vec[0]));
*
*/
return(xEsimateV);
}
private void Predict()
{
xEsimateV = Matrix2x2.MulVector2(vTrans, xEsimateSaveV);
// F*p*F' + Q
double[] mTrans1 = Matrix2x2.MulVector2(vTrans, vError);
//Matrix2x2 mTrans2 = MatrixTrans.Transpone();
double[] mTrans2 = Matrix2x2.MulVector2(mTrans1, vTrans);
vError = Matrix2x2.SumVectors(mTrans2, vNoiseProcess);
}
public static Matrix2x2 Sub(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 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 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)
{
if (bIsFirst == true)
{
xEsimateSave[0] = vec[0];
xEsimateSave[1] = vec[1];
bIsFirst = false;
}
if (vec[0] == -1)
{
vec[0] = xEsimateSave[0];
vec[1] = xEsimateSave[1];
}
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);
}
