double
probability_between_move_olds( logtools_rmove2_t move1, logtools_rmove2_t move2 )
{
logtools_vector2_t v1, v2;
double mval1 = 50.0;
double mval2 = deg2rad(20.0);
double val1, val2, ret;
v1.x = move1.forward;
v1.y = move1.sideward;
v2.x = move2.forward;
v2.y = move2.sideward;
val1 = logtools_vector2_distance( v1, v2 );
val2 = fabs( compute_orientation_diff( move1.rotation,
move2.rotation ) );
if (val1>mval1) {
val1 = mval1;
}
if (val2>mval2) {
val2 = mval2;
}
ret = ( ((mval1-val1)/mval1) + ((mval2-val2)/mval2) )/2.0;
return( ret );
}
double
probability_between_moves( logtools_rmove2_t move1, logtools_rmove2_t move2 )
{
double sum = 0.0;
sum += log( EPSILON +
logtools_gauss_function( fabs( move1.forward-move2.forward ),
0, settings.motion_model.forward ));
sum += log( EPSILON +
logtools_gauss_function( fabs( move1.sideward-move2.sideward ),
0, settings.motion_model.sideward));
sum += log( EPSILON +
logtools_gauss_function( fabs( compute_orientation_diff( move1.rotation,
move2.rotation )),
0, settings.motion_model.rotation ));
return( sum );
}
logtools_rmove2_t
logtools_movement2_between_rpos2( logtools_rpos2_t start, logtools_rpos2_t end )
{
logtools_rmove2_t move;
/* compute forward and sideward sensing_MOVEMENT */
move.forward =
+ (end.y - start.y) * sin(start.o)
+ (end.x - start.x) * cos(start.o);
move.sideward =
- (end.y - start.y) * cos(start.o)
+ (end.x - start.x) * sin(start.o);
move.rotation = compute_orientation_diff( end.o, start.o );
return( move );
}
