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