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