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