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