示例#1
0
/*
 * 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 ));
}
示例#2
0
// -----------------------------------------------------------------------------
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 );
}
示例#3
0
/*
 *--------------------------------------------------------------------------------------
 *       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  ----- */