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