/* * A tadpole with a full propagator and with 1 LV vertex */ main() { page pg; FeynDiagram fd(pg); #define RAD 2.5 fd.line_thickness.set(0.15); fd.vertex_thickness.set(0.15); /* define the left and the right external points */ xy el(-TADPOLELEG, 0), er(6, 0); /* the LV vertex */ xy arcpt1(RAD, 0); xy arcpt2(0, -RAD); /* the ordinary SQED vertex */ vertex_circlecross v1(fd, -RAD, 0); vertex v2(fd, RAD, 0); vertex fict(fd, -RAD - 0.5, 0); /* for a nicer picture */ /* photon line propagators */ line_wiggle ph1(fd, el, fict); line_plain f1(fd, v1, v1); f1.thickness.set(FULLPROPTHICK); f1.arrowon.setfalse(); // line_plain f2(fd, v2, v1); /* stretch it to be an arc */ f1.arcthru(arcpt1); // f2.arcthru(arcpt2); pg.output(); return 0; }
void KalmanModel::Predict(Localization::KMotionModel & MotionModel){ float tmpDist, tmpDir, tmpRot; tmpDir = MotionModel.Direction.val; tmpDist = MotionModel.Distance.val; tmpRot = MotionModel.Rotation.val; matrix6_6 gt; matrix6_3 vt; matrix3_3 mt; matrix6_6 fict; fict.zero(); // Small amount of noise for error parameters fict(0,0) = 4e-6; fict(1,1) = 4e-6; fict(2,2) = 4e-6; fict(3,3) = tmpDist * tmpDist * 0.02; fict(4,4) = tmpDist * tmpDist * 0.02; fict(5,5) = tmpDist * tmpDist * 0.002; gt.zero(); mt.zero(); vt.zero(); // Linearizing odometry model gt(0,0) = 1; gt(0,1) = 0; gt(0,2) = - state(3,0) * tmpDist * sin(state(2,0) + tmpDir); gt(0,3) = tmpDist * cos(state(2,0) + tmpDir); gt(0,4) = 0; gt(0,5) = 0; gt(1,0) = 0; gt(1,1) = 1; gt(1,2) = state(3,0) * tmpDist * cos(state(2,0) + tmpDir); gt(1,3) = tmpDist * sin(state(2,0) + tmpDir); gt(1,4) = 0; gt(1,5) = 0; gt(2,0) = 0; gt(2,1) = 0; gt(2,2) = 1; gt(2,3) = 0; gt(2,4) = tmpDist; gt(2,5) = tmpRot; ///////////////////////////////// gt(3,0) = 0; gt(3,1) = 0; gt(3,2) = 0; gt(3,3) = 1; gt(3,4) = 0; gt(3,5) = 0; gt(4,0) = 0; gt(4,1) = 0; gt(4,2) = 0; gt(4,3) = 0; gt(4,4) = 1; gt(4,5) = 0; gt(5,0) = 0; gt(5,1) = 0; gt(5,2) = 0; gt(5,3) = 0; gt(5,4) = 0; gt(5,5) = 1; //gt.prettyPrint(); mt(0,0) = tmpDist * tmpDist * 0.6 * 0.6; mt(1,1) = tmpDist * tmpDist * 0.6 * 0.6; mt(2,2) = tmpRot * tmpRot * 0.3 * 0.3 + tmpDist * tmpDist * 0.1 ; //mt.prettyPrint() vt(0,0) = - state(3,0) * tmpDist * sin(state(2,0) + tmpDir); vt(0,1) = state(3,0) * cos(state(2,0) + tmpDir); vt(0,2) = 0; vt(1,0) = state(3,0) * tmpDist * cos(state(2,0) + tmpDir); vt(1,1) = state(3,0) * sin(state(2,0) + tmpDir); vt(1,2) = 0; vt(2,0) = 0; vt(2,1) = state(4,0); vt(2,2) = state(5,0); vt(3,0) = 0; vt(3,1) = 0; vt(3,2) = 0; vt(4,0) = 0; vt(4,1) = 0; vt(4,2) = 0; vt(5,0) = 0; vt(5,1) = 0; vt(5,2) = 0; //vt.prettyPrint(); //Compute new state and variance state(0,0) += cos(tmpDir + state(2,0)) * tmpDist * state(3,0) ; state(1,0) += sin(tmpDir + state(2,0)) * tmpDist * state(3,0); state(2,0) += state(5,0) * tmpRot + state(4,0) * tmpDist; state(2,0) = wrapTo0_2Pi(state(2,0)); //odometry.prettyPrint(); //state.prettyPrint(); var =(((gt.slow_mult(var)).slow_mult(gt.transp())).add((vt.slow_mult(mt)).slow_mult(vt.transp()))).add(fict); //var.prettyPrint(); }