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