/*
* Motion model for an Ackerman drive
* pSteeringAngle : Steering angle of the front wheels
* (90deg is forward direction)
* pDistance : Driven distance with pSteeringAngle
* dx : change of x position
* dy : change of y position
* alpha : change of heading
* R : turning radius
*/
Matx33d Map::computeMotionUpdate( double pSteeringAngle, double pDistance )
{
double r;
double icr_x, icr_y;
double alpha;
Matx33d R, T1, T2;
if (abs(pSteeringAngle - 90) < 1)
{
alpha = 0;
r = 0;
R = Matx33d( 1, 0, 0,
0, 1, pDistance * _mToP,
0, 0, 1);
}
else
{
double _phi = abs( pSteeringAngle - 90. ) * PI / 180.; // in rad
r = _achsisDistance / tan(_phi); // in m
r = pSteeringAngle > 90. ? -r : r;
alpha = pDistance / r; // in radians
R = Matx33d( cos(alpha), -sin(alpha), 0,
sin(alpha), cos(alpha), 0,
0, 0, 1);
// printf("Main: r: %f a: %f\n", r, alpha);
}
icr_x = _carPos[0] - ( r * _mToP );
icr_y = _carPos[1];
// printf("x: %f, y: %f, r:%f\n", icr_x, icr_y, r*_mToP);
T1 = Matx33d( 1, 0, -icr_x,
0, 1, -icr_y,
0, 0, 1);
T2 = Matx33d( 1, 0, icr_x,
0, 1, icr_y,
0, 0, 1);
return (T2 * ( R * T1 ));
}
// -----------------------------------------------------------------------------
Map::Map( Vec2d dimensions, Vec3d carPosition, int mToP, double achsisDistance,
double associationDistance) :
_dimensions(dimensions),
_carPos(carPosition),
_mToP(mToP),
_achsisDistance(achsisDistance),
_assocDistance(associationDistance)
{
Q = Matx33d( 0.1, 0.2, 0,
0.2, 0.1, 0,
0, 0, 1 );
I = Matx33d( 1, 0, 0,
0, 1, 0,
0, 0, 1 );
}
/*
*--------------------------------------------------------------------------------------
* Class: States
* Method: States :: dynamics
* Description: Apply motion model to state and return predicted state.
*--------------------------------------------------------------------------------------
*/
States
States::dynamics ( const Sensors& s )
{
States predicted_state;
Matx33d A;
cv::Vec3d w;
Matx33d Rb2w, Rw2b;
Rb2w = s.quaternion.rotation();
Rw2b = Rb2w.t();
w =cv::Vec3d(s.angular_velocity);
Vec3d gw(0,0,GRAVITY);
A = Matx33d( 0, -w[2], w[1],
w[2], 0, -w[0],
-w[1], w[0], 0 );
// Generalized matrix multiplication
gemm( Rb2w, V, 1, Mat(), 0, predicted_state.X );
gemm( -A, V, 1, s.acceleration, 1, predicted_state.V );
gemm( Rw2b, gw, -1, predicted_state.V, 1, predicted_state.V);
Fiter pib=features.begin();
for( int i=0; pib!=features.end(); ++pib,++i )
{
Feature fi;
cv::Vec3d bp;
bp = (*pib)->get_body_position();
fi.set_body_position( cv::Vec3d(
(-V[1] + bp[0]*V[0])*bp[2] + bp[1]*w[0] - (1 + bp[0]*bp[0])*w[2] + bp[0]*bp[1]*w[1],
(-V[2] + bp[1]*V[0])*bp[2] - bp[0]*w[0] + (1 + bp[1]*bp[1])*w[1] - bp[0]*bp[1]*w[2],
(-w[2] * bp[0]+w[1] *bp[1])* bp[2]+V[0] * bp[2]*bp[2])
);
predicted_state.addFeature(fi);
}
V-=b;
b=cv::Vec3d(0,0,0);
return predicted_state;
} /* ----- end of method States::dynamics ----- */