예제 #1
0
파일: ocp.cpp 프로젝트: GCTMODS/nmpc
DeltaVector OptimalControlProblem::state_to_delta(
const StateVector &s1, const StateVector &s2) {
    DeltaVector delta;

    delta.segment<6>(0) = s2.segment<6>(0) - s1.segment<6>(0);

    /*
    In order to increase the linearity of the problem and avoid quaternion
    normalisation issues, we calculate the difference between attitudes as a
    3-vector of Modified Rodrigues Parameters (MRP).
    */
    Quaternionr err_q = (Quaternionr(s2.segment<4>(6)) *
        Quaternionr(s1.segment<4>(6)).conjugate());

    if(err_q.w() < 0) {
        err_q = Quaternionr(-err_q.w(), -err_q.x(), -err_q.y(), -err_q.z());
    }

    delta.segment<3>(6) = NMPC_MRP_F *
        (err_q.vec() / (NMPC_MRP_A + err_q.w()));

    delta.segment<3>(9) = s2.segment<3>(10) - s1.segment<3>(10);

    return delta;
}
예제 #2
0
파일: state.cpp 프로젝트: GCTMODS/nmpc
/*
Runs the kinematics model on the state vector and returns a vector with the
derivative of each components (except for the accelerations, which must be
calculated directly using a dynamics model).
Contents are as follows:
    - Rate of change in position (3-vector, m/s, NED frame)
    - Rate of change in linear velocity (3-vector, m/s^2, NED frame)
    - Rate of change in attitude (quaternion (x, y, z, w), 1/s, body frame)
    - Rate of change in angular velocity (3-vector, rad/s^2, body frame)
*/
const StateVectorDerivative State::model(ControlVector c, DynamicsModel *d) {
    StateVectorDerivative output;

    AccelerationVector a = d->evaluate(*this, c);

    /* Calculate change in position. */
    output.segment<3>(0) << velocity();

    /* Calculate change in velocity. */
    Quaternionr attitude_q = Quaternionr(attitude());
    output.segment<3>(3) = attitude_q.conjugate() * a.segment<3>(0);

    /* Calculate change in attitude. */
    Eigen::Matrix<real_t, 4, 1> omega_q;
    omega_q << angular_velocity(), 0;

    attitude_q = Quaternionr(omega_q).conjugate() * attitude_q;
    output.segment<4>(6) << attitude_q.vec(), attitude_q.w();
    output.segment<4>(6) *= 0.5;

    /* Calculate change in angular velocity (just angular acceleration). */
    output.segment<3>(10) = a.segment<3>(3);

    return output;
}
예제 #3
0
파일: interface.cpp 프로젝트: nuaa/ukf
void ukf_set_params(struct ioboard_params *in) {
    model = IOBoardModel(
        Quaternionr(
            in->accel_orientation[0],
            in->accel_orientation[1],
            in->accel_orientation[2],
            in->accel_orientation[3]),
        Vector3r(
            in->accel_offset[0],
            in->accel_offset[1],
            in->accel_offset[2]),
        Quaternionr(
            in->gyro_orientation[0],
            in->gyro_orientation[1],
            in->gyro_orientation[2],
            in->gyro_orientation[3]),
        Quaternionr(
            in->mag_orientation[0],
            in->mag_orientation[1],
            in->mag_orientation[2],
            in->mag_orientation[3]),
        Vector3r(
            in->mag_field[0],
            in->mag_field[1],
            in->mag_field[2]));
}
예제 #4
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, Instantiation) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));

    EXPECT_EQ(test.collate(), MeasurementVector());
}
예제 #5
0
파일: ukf.cpp 프로젝트: nikhil9/ukf
/* Follows section 3.1, however we're using the scaled unscented transform. */
void UnscentedKalmanFilter::create_sigma_points() {
    Eigen::Matrix<real_t, UKF_STATE_DIM, UKF_STATE_DIM> S;

    AssertNormalized(Quaternionr(state.attitude()));

    /* Add the process noise before calculating the square root. */
    state_covariance.diagonal() += process_noise_covariance;
    state_covariance *= UKF_DIM_PLUS_LAMBDA;

    AssertPositiveDefinite(state_covariance);

    /* Compute the 'square root' of the covariance matrix. */
    S = state_covariance.llt().matrixL();

    /* Create centre sigma point. */
    sigma_points.col(0) = state;

    /* For each column in S, create the two complementary sigma points. */
    for(uint8_t i = 0; i < UKF_STATE_DIM; i++) {
        /*
        Construct error quaternions using the MRP method, equation 34 from the
        Markley paper.
        */
        Vector3r d_p = S.col(i).segment<3>(9);
        real_t x_2 = d_p.squaredNorm();
        real_t err_w = (-UKF_MRP_A * x_2 +
            UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
            (UKF_MRP_F_2 + x_2);
        Vector3r err_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + err_w)) * d_p;
        Quaternionr noise;
        noise.vec() = err_xyz;
        noise.w() = err_w;

        Quaternionr temp;

        /* Create positive sigma point. */
        sigma_points.col(i+1).segment<9>(0) =
            state.segment<9>(0) + S.col(i).segment<9>(0);
        temp = noise * Quaternionr(state.attitude());
        AssertNormalized(temp);
        sigma_points.col(i+1).segment<4>(9) << temp.vec(), temp.w();
        sigma_points.col(i+1).segment<12>(13) =
            state.segment<12>(13) + S.col(i).segment<12>(12);

        /* Create negative sigma point. */
        sigma_points.col(i+1+UKF_STATE_DIM).segment<9>(0) =
            state.segment<9>(0) - S.col(i).segment<9>(0);
        temp = noise.conjugate() * Quaternionr(state.attitude());
        AssertNormalized(temp);
        sigma_points.col(i+1+UKF_STATE_DIM).segment<4>(9) <<
            temp.vec(), temp.w();
        sigma_points.col(i+1+UKF_STATE_DIM).segment<12>(13) =
            state.segment<12>(13) - S.col(i).segment<12>(12);
    }
}
예제 #6
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetPitotTAS) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    MeasurementVector target(1);

    test.set_pitot_tas(1);
    target << 1;

    EXPECT_EQ(test.collate(), target);
}
예제 #7
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetBarometerAMSL) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    MeasurementVector target(1);

    test.set_barometer_amsl(1);
    target << 1;

    EXPECT_EQ(test.collate(), target);
}
예제 #8
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetMagnetometer) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    MeasurementVector target(3);

    test.set_magnetometer(Vector3r(1, 2, 3));
    target << 1, 2, 3;

    EXPECT_EQ(test.collate(), target);
}
예제 #9
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetGPSVelocity) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    MeasurementVector target(3);

    test.set_gps_velocity(Vector3r(1, 2, 3));
    target << 1, 2, 3;

    EXPECT_EQ(test.collate(), target);
}
예제 #10
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetThenClear) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));

    test.set_accelerometer(Vector3r(1, 2, 3));
    test.set_gyroscope(Vector3r(4, 5, 6));
    test.set_barometer_amsl(7);
    test.clear();

    EXPECT_EQ(test.collate(), MeasurementVector());
}
예제 #11
0
/* this function is called from field renderers for all field nodes */
void Renderer::setNodeGlData(const shared_ptr<Node>& n){
	bool scaleRotations=(rotScale!=1.0 && scaleOn);
	bool scaleDisplacements=(dispScale!=Vector3r::Ones() && scaleOn);
	const bool isPeriodic=scene->isPeriodic;

	if(!n->hasData<GlData>()) n->setData<GlData>(make_shared<GlData>());
	GlData& gld=n->getData<GlData>();
	// inside the cell if periodic, same as pos otherwise
	Vector3r cellPos=(!isPeriodic ? n->pos : scene->cell->canonicalizePt(n->pos,gld.dCellDist)); 
	bool rendered=!pointClipped(cellPos);
	// this encodes that the node is clipped
	if(!rendered){ gld.dGlPos=Vector3r(NaN,NaN,NaN); return; }
	if(setRefNow || isnan(gld.refPos[0])) gld.refPos=n->pos;
	if(setRefNow || isnan(gld.refOri.x())) gld.refOri=n->ori;
	const Vector3r& pos=n->pos; const Vector3r& refPos=gld.refPos;
	const Quaternionr& ori=n->ori; const Quaternionr& refOri=gld.refOri;
	// if no scaling and no periodic, return quickly
	if(!(scaleDisplacements||scaleRotations||isPeriodic)){ gld.dGlPos=Vector3r::Zero(); gld.dGlOri=Quaternionr::Identity(); return; }
	// apply scaling
	gld.dGlPos=cellPos-n->pos;
	// add scaled translation to the point of reference
	if(scaleDisplacements) gld.dGlPos+=((dispScale-Vector3r::Ones()).array()*Vector3r(pos-refPos).array()).matrix(); 
	if(!scaleRotations) gld.dGlOri=Quaternionr::Identity();
	else{
		Quaternionr relRot=refOri.conjugate()*ori;
		AngleAxisr aa(relRot);
		aa.angle()*=rotScale;
		gld.dGlOri=Quaternionr(aa);
	}
}
예제 #12
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetMultipleInOrder) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    MeasurementVector target(7);

    test.set_accelerometer(Vector3r(1, 2, 3));
    test.set_gyroscope(Vector3r(4, 5, 6));
    test.set_barometer_amsl(7);
    target << 1, 2, 3, 4, 5, 6, 7;

    EXPECT_EQ(test.collate(), target);
}
예제 #13
0
void OpenGLRenderer::setBodiesDispInfo(){
	if(scene->bodies->size()!=bodyDisp.size()) {
		bodyDisp.resize(scene->bodies->size());
		for (unsigned k=0; k<scene->bodies->size(); k++) bodyDisp[k].hidden=0;}
	bool scaleRotations=(rotScale!=1.0);
	bool scaleDisplacements=(dispScale!=Vector3r::Ones());
	FOREACH(const shared_ptr<Body>& b, *scene->bodies){
		if(!b || !b->state) continue;
		size_t id=b->getId();
		const Vector3r& pos=b->state->pos; const Vector3r& refPos=b->state->refPos;
		const Quaternionr& ori=b->state->ori; const Quaternionr& refOri=b->state->refOri;
		Vector3r cellPos=(!scene->isPeriodic ? pos : scene->cell->wrapShearedPt(pos)); // inside the cell if periodic, same as pos otherwise
		bodyDisp[id].isDisplayed=!pointClipped(cellPos);
		// if no scaling and no periodic, return quickly
		if(!(scaleDisplacements||scaleRotations||scene->isPeriodic)){ bodyDisp[id].pos=pos; bodyDisp[id].ori=ori; continue; }
		// apply scaling
		bodyDisp[id].pos=cellPos; // point of reference (inside the cell for periodic)
		if(scaleDisplacements) bodyDisp[id].pos+=dispScale.cwiseProduct(Vector3r(pos-refPos)); // add scaled translation to the point of reference
		if(!scaleRotations) bodyDisp[id].ori=ori;
		else{
			Quaternionr relRot=refOri.conjugate()*ori;
			AngleAxisr aa(relRot);
			aa.angle()*=rotScale;
			bodyDisp[id].ori=refOri*Quaternionr(aa);
		}
	}
}
예제 #14
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, SetMultipleOutOfOrder) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    MeasurementVector target(10);

    test.set_magnetometer(Vector3r(4, 5, 6));
    test.set_accelerometer(Vector3r(1, 2, 3));
    test.set_barometer_amsl(10);
    test.set_gps_velocity(Vector3r(7, 8, 9));
    target << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;

    EXPECT_EQ(test.collate(), target);
}
예제 #15
0
파일: ScGeom.cpp 프로젝트: DEMANY/trunk
void ScGeom6D::initRotations(const State& state1, const State& state2)
{
	initialOrientation1	= state1.ori;
	initialOrientation2	= state2.ori;
	twist=0;
	bending=Vector3r::Zero();
	twistCreep=Quaternionr(1.0,0.0,0.0,0.0);
}
예제 #16
0
파일: NodeGlRep.cpp 프로젝트: CrazyHeex/woo
void TensorGlRep::render(const shared_ptr<Node>& node, const GLViewInfo* viewInfo){

	const int circDiv=20;
	static gleDouble circ[circDiv+2][3]={};

	if(circ[0][0]==0){
		gleSetNumSides(10);
		Real step=2*M_PI/circDiv;
		for(int i=-1; i<circDiv+1; i++){
			circ[i+1][0]=cos(i*step);
			circ[i+1][1]=sin(i*step);
			circ[i+1][2]=0;
		}
	}

	if(range && !skewRange) skewRange=range;

	Vector3r pos=node->pos+(node->hasData<GlData>()?node->getData<GlData>().dGlPos:Vector3r::Zero());

	for(int i:{0,1,2}){
		Vector3r color=(range?range->color(eigVal[i]):CompUtils::scalarOnColorScale(eigVal[i],-1,1));
		if(isnan(color.maxCoeff())) continue;
		Real mxNorm=(range?range->maxAbs(eigVal[i]):1);
		Real len=relSz*viewInfo->sceneRadius;
		len*=isnan(scaleExp)?abs(eigVal[i]/mxNorm):pow(abs(eigVal[i])/mxNorm,scaleExp);
		glColor3v(color);
		Vector3r dx(len*eigVec.col(i));
		// arrows which go towards each other for negative eigenvalues, and away from each other for positive ones
		GLUtils::GLDrawArrow(pos+(eigVal[i]>0?Vector3r::Zero():dx),pos+(eigVal[i]>0?dx:Vector3r::Zero()),color);
		GLUtils::GLDrawArrow(pos-(eigVal[i]>0?Vector3r::Zero():dx),pos-(eigVal[i]>0?dx:Vector3r::Zero()),color);

		// draw circular arrow to show skew components, in the half-height
		// compute it in the xy plane, transform coords instead
		Real maxSkew=(skewRange?skewRange->maxAbs(skew[i]):1);
		// if(abs(skew[i])<.05*maxSkew) continue;

		int nPts=int((abs(skew[i])/maxSkew)*circDiv*.5/* show max skew as .5*2π rad */-.5/*round evenly, but exclude one segment for arrow head*/);
		if(nPts>circDiv-2) nPts=circDiv-2;
		if(nPts<=0) continue;
		Real torRad1=(skewRelSz>0?skewRelSz:relSz)*viewInfo->sceneRadius*(abs(skew[i])/maxSkew);
		Real torDist=0*.3*torRad1; // draw both arcs in-plane
		Real torRad2=torRad1*.1;
		glColor3v(skewRange?skewRange->color(skew[i]):CompUtils::scalarOnColorScale(skew[i],-1,1));
		for(int j:{0,1,2}){
			glPushMatrix();
				Eigen::Affine3d T=Eigen::Affine3d::Identity();
				T.translate(pos+(j==0?1:-1)*eigVec.col(i)*torDist).rotate(Quaternionr().setFromTwoVectors(Vector3r::UnitZ(),eigVec.col(i)*(skew[i]>0?-1:1))).scale(torRad1);
				if(j==1) T.rotate(AngleAxisr(M_PI,Vector3r::UnitZ()));
				//GLUtils::setLocalCoords(pos+(j==0?1:-1)*eigVec.col(i)*torDist,
				glMultMatrixd(T.data());
				// since we scaled coords to transform unit circle coords to our radius, we will need to scale dimensions now by 1/torRad1
				glePolyCylinder(nPts+2,circ,/* use current color*/NULL,torRad2*(1/torRad1));
				gleDouble headRad[]={2*torRad2*(1/torRad1),2*torRad2*(1/torRad1),0,0,0};
				glePolyCone(4,(gleDouble(*)[3])&(circ[nPts-1]),/*use current color*/NULL,headRad);
			glPopMatrix();
		}
	}
}
예제 #17
0
파일: GLUtils.cpp 프로젝트: Mikelian/trunk
/*! Draws a 3D arrow between the 3D point \p from and the 3D point \p to, both defined in the
current ModelView coordinates system.

See drawArrow(float length, float radius, int nbSubdivisions) for details. */
void GLUtils::QGLViewer::drawArrow(const Vector3r& from, const Vector3r& to, float radius, int nbSubdivisions)
{
	glPushMatrix();
	glTranslatef(from[0],from[1],from[2]);
	Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),to-from));
	//glMultMatrixd(q.toRotationMatrix().data());
	glMultMatrix(q.toRotationMatrix().data());
	drawArrow((to-from).norm(), radius, nbSubdivisions);
	glPopMatrix();
}
예제 #18
0
파일: ukf.cpp 프로젝트: nikhil9/ukf
/*
The final update step is calculated using section 3.6, specifically with
equations 74 and 75.
*/
void UnscentedKalmanFilter::aposteriori_estimate() {
    ProcessCovariance update_temp;

    /*
    The update_temp vector will contain the attitude update as a vector, which
    we need to apply the to quaternion section of the state vector.
    */
    update_temp = kalman_gain * innovation;

    /*
    Update the attitude quaternion from the MRP vector, equation 45 from the
    Markley paper.
    */
    Vector3r d_p = update_temp.segment<3>(9);
    real_t x_2 = d_p.squaredNorm();
    real_t d_q_w = (-UKF_MRP_A * x_2 +
        UKF_MRP_F * std::sqrt(UKF_MRP_F_2 + ((real_t)1.0 - UKF_MRP_A_2) * x_2)) /
        (UKF_MRP_F_2 + x_2);
    Vector3r d_q_xyz = (((real_t)1.0 / UKF_MRP_F) * (UKF_MRP_A + d_q_w)) * d_p;
    Quaternionr d_q;
    d_q.vec() = d_q_xyz;
    d_q.w() = d_q_w;

    Quaternionr update_q = d_q *
        Quaternionr(sigma_points.col(0).segment<4>(9));
    state.attitude() << update_q.vec(), update_q.w();

    AssertNormalized(Quaternionr(state.attitude()));

    /* The rest of the update step is very straightforward. */
    state.segment<9>(0) = apriori_mean.segment<9>(0) +
        update_temp.segment<9>(0);
    state.segment<12>(13) = apriori_mean.segment<12>(13) +
        update_temp.segment<12>(12);

    /*
    And the state covariance update from equation 75, no quaternion
    manipulation necessary.
    */
    state_covariance = apriori_covariance -
        (kalman_gain * innovation_covariance * kalman_gain.transpose());
}
예제 #19
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictGyroscope) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    State test_state;
    test_state << 0, 0, 0,
            0, 0, 0,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            4, 5, 6,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0;

    test.set_gyroscope(Vector3r(0, 0, 0));

    EXPECT_EQ(test.predict(test_state), Vector3r(4, 5, 6));
}
예제 #20
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictBarometerAMSL) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(7, 8, 9));
    State test_state;
    test_state << -37, 145, 150,
            7, 8, 9,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            4, 5, 6,
            0, 0, 0,
            1, 2, 3,
            0, 0, 0;

    test.set_barometer_amsl(0);

    EXPECT_TRUE(abs(test.predict(test_state)(0) - 150) < 0.1);
}
예제 #21
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictPitotTAS) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(7, 8, 9));
    State test_state;
    test_state << 0, 0, 0,
            7, 8, 9,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            4, 5, 6,
            0, 0, 0,
            1, 2, 3,
            0, 0, 0;

    test.set_pitot_tas(0);

    EXPECT_EQ(test.predict(test_state)(0), 6);
}
예제 #22
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictMagnetometer) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(7, 8, 9));
    State test_state;
    test_state << 0, 0, 0,
            0, 0, 0,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            4, 5, 6,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0;

    test.set_magnetometer(Vector3r(0, 0, 0));

    EXPECT_TRUE(test.predict(test_state).isApprox(
        Vector3r(7, -9, 8), 0.001));
}
예제 #23
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictGPSVelocity) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(7, 8, 9));
    State test_state;
    test_state << 0, 0, 0,
            7, 8, 9,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            4, 5, 6,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0;

    test.set_gps_velocity(Vector3r(0, 0, 0));

    EXPECT_TRUE(test.predict(test_state).isApprox(
        Vector3r(7, 8, 9), 0.001));
}
예제 #24
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictGPSPosition) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(7, 8, 9));
    State test_state;
    test_state << -37, 145, 0,
            0, 0, 0,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            4, 5, 6,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0;

    test.set_gps_position(Vector3r(0, 0, 0));

    EXPECT_TRUE(test.predict(test_state).isApprox(
        Vector3r(-37, 145, 0), 0.001));
}
예제 #25
0
void Membrane::setRefConf(){
	if(!node) node=make_shared<Node>();
	// set initial node position and orientation
	node->pos=this->getCentroid();
	// for orientation, there is a few choices which one to pick
	enum {INI_CS_SIMPLE=0,INI_CS_NODE0_AT_X};
	const int ini_cs=INI_CS_NODE0_AT_X;
	//const int ini_cs=INI_CS_SIMPLE;
	switch(ini_cs){
		case INI_CS_SIMPLE:
			node->ori.setFromTwoVectors(Vector3r::UnitZ(),this->getNormal());
			break;
		case INI_CS_NODE0_AT_X:{
			Matrix3r T;
			T.col(0)=(nodes[0]->pos-node->pos).normalized(); // QQQ: row->col
			T.col(2)=this->getNormal();
			T.col(1)=T.col(2).cross(T.col(0));
			assert(T.col(0).dot(T.col(2))<1e-12);
			node->ori=Quaternionr(T);
			break;
		}
	};
	// reference nodal positions
	for(int i:{0,1,2}){
		// Vector3r nl=node->ori.conjugate()*(nodes[i]->pos-node->pos); // QQQ
		Vector3r nl=node->glob2loc(nodes[i]->pos);
		assert(nl[2]<1e-6*(max(abs(nl[0]),abs(nl[1])))); // z-coord should be zero
		refPos.segment<2>(2*i)=nl.head<2>();
	}
	// reference nodal rotations
	refRot.resize(3);
	for(int i:{0,1,2}){
		// facet node orientation minus vertex node orientation, in local frame (read backwards)
		/// QQQ: refRot[i]=node->ori*nodes[i]->ori.conjugate();
		refRot[i]=nodes[i]->ori.conjugate()*node->ori;
		//LOG_WARN("refRot["<<i<<"]="<<AngleAxisr(refRot[i]).angle()<<"/"<<AngleAxisr(refRot[i]).axis().transpose());
	};
	// set displacements to zero
	uXy=phiXy=Vector6r::Zero();
	#ifdef MEMBRANE_DEBUG_ROT
		drill=Vector3r::Zero();
		currRot.resize(3);
	#endif
	// delete stiffness matrices to force their re-creating
	KKcst.resize(0,0); 
	KKdkt.resize(0,0);
};
예제 #26
0
void woo::Volumetric::computePrincipalAxes(const Real& M, const Vector3r& Sg, const Matrix3r& Ig, Vector3r& pos, Quaternionr& ori, Vector3r& inertia){
	assert(M>0); // LOG_TRACE("M=\n"<<M<<"\nIg=\n"<<Ig<<"\nSg=\n"<<Sg);
	// clump's centroid
	pos=Sg/M;
	// this will calculate translation only, since rotation is zero
	Matrix3r Ic_orientG=inertiaTensorTranslate(Ig, -M /* negative mass means towards centroid */, pos); // inertia at clump's centroid but with world orientation
	//LOG_TRACE("Ic_orientG=\n"<<Ic_orientG);
	Ic_orientG(1,0)=Ic_orientG(0,1); Ic_orientG(2,0)=Ic_orientG(0,2); Ic_orientG(2,1)=Ic_orientG(1,2); // symmetrize
	Eigen::SelfAdjointEigenSolver<Matrix3r> decomposed(Ic_orientG);
	const Matrix3r& R_g2c(decomposed.eigenvectors());
	//cerr<<"R_g2c:"<<endl<<R_g2c<<endl;
	// has NaNs for identity matrix??
	//LOG_TRACE("R_g2c=\n"<<R_g2c);
	// set quaternion from rotation matrix
	ori=Quaternionr(R_g2c); ori.normalize();
	inertia=decomposed.eigenvalues();
}
예제 #27
0
        ImageResponse(const msr::airlib::VehicleCameraBase::ImageResponse& s)
        {
            pixels_as_float = s.pixels_as_float;
            
            image_data_uint8 = s.image_data_uint8;
            image_data_float = s.image_data_float;

            //TODO: remove bug workaround for https://github.com/rpclib/rpclib/issues/152
            if (image_data_uint8.size() == 0)
                image_data_uint8.push_back(0);
            if (image_data_float.size() == 0)
                image_data_float.push_back(0);

            camera_position = Vector3r(s.camera_position);
            camera_orientation = Quaternionr(s.camera_orientation);
            time_stamp = s.time_stamp;
            message = s.message;
            compress = s.compress;
            width = s.width;
            height = s.height;
            image_type = s.image_type;
        }
예제 #28
0
	void Gl1_PolyhedraPhys::go(const shared_ptr<IPhys>& ip, const shared_ptr<Interaction>& i, const shared_ptr<Body>& b1, const shared_ptr<Body>& b2, bool wireFrame){
		if(!gluQuadric){ gluQuadric=gluNewQuadric(); if(!gluQuadric) throw runtime_error("Gl1_PolyhedraPhys::go unable to allocate new GLUquadric object (out of memory?)."); }
		PolyhedraPhys* np=static_cast<PolyhedraPhys*>(ip.get());
		shared_ptr<IGeom> ig(i->geom); if(!ig) return; // changed meanwhile?
		PolyhedraGeom* geom=YADE_CAST<PolyhedraGeom*>(ig.get());
		Real fnNorm=np->normalForce.dot(geom->normal);
		if((signFilter>0 && fnNorm<0) || (signFilter<0 && fnNorm>0)) return;
		int fnSign=fnNorm>0?1:-1;
		fnNorm=std::abs(fnNorm);
		Real radiusScale=1.;
		maxFn=max(fnNorm,maxFn);
		Real realMaxRadius;
		if(maxRadius<0){
			refRadius=min(0.03,refRadius);
			realMaxRadius=refRadius;
		}
		else realMaxRadius=maxRadius;
		Real radius=radiusScale*realMaxRadius*(fnNorm/maxFn); 
		if (radius<=0.) radius = 1E-8;
		Vector3r color=Shop::scalarOnColorScale(fnNorm*fnSign,-maxFn,maxFn);
		
		Vector3r p1=b1->state->pos, p2=b2->state->pos;
		Vector3r relPos;
		relPos=p2-p1;
		Real dist=relPos.norm();
				
		glDisable(GL_CULL_FACE); 
		glPushMatrix();
			glTranslatef(p1[0],p1[1],p1[2]);
			Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),relPos/dist /* normalized */));
			// using Transform with OpenGL: http://eigen.tuxfamily.org/dox/TutorialGeometry.html
			//glMultMatrixd(Eigen::Affine3d(q).data());
			glMultMatrix(Eigen::Transform<Real,3,Eigen::Affine>(q).data());
			glColor3v(color);
			gluCylinder(gluQuadric,radius,radius,dist,slices,stacks);
		glPopMatrix();
	}
예제 #29
0
파일: test_sensors.cpp 프로젝트: nuaa/ukf
TEST(IOBoardModelTest, PredictAccelerometer) {
    IOBoardModel test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    State test_state;
    test_state << 0, 0, 0,
            0, 0, 0,
            1, 2, 3,
            0.7071, 0, 0, 0.7071,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0;
    Eigen::Matrix<real_t, 6, 1> target;

    test.set_accelerometer(Vector3r(0, 0, 0));

    target << 1, 2, 3, 0, G_ACCEL, 0;
    EXPECT_TRUE(test.predict(test_state).isApprox(target, 0.001));

    test = IOBoardModel(
        Quaternionr(1, 0, 0, 0),
        Vector3r(1, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Quaternionr(1, 0, 0, 0),
        Vector3r(0, 0, 0));
    test_state << 0, 0, 0,
            0, 0, 0,
            0, 0, 0,
            0, 0, 0, 1,
            0, 0, 1,
            0, 0, 1,
            0, 0, 0,
            0, 0, 0;

    test.set_accelerometer(Vector3r(0, 0, 0));

    target << -1, 1, 0, 0, 0, -G_ACCEL;
    EXPECT_TRUE(test.predict(test_state).isApprox(target, 0.001));
}
예제 #30
0
void Gl1_Membrane::go(const shared_ptr<Shape>& sh, const Vector3r& shift, bool wire2, const GLViewInfo& viewInfo){
	Gl1_Facet::go(sh,shift,/*don't force wire rendering*/ wire2,viewInfo);
	if(Renderer::fastDraw) return;
	Membrane& ff=sh->cast<Membrane>();
	if(!ff.hasRefConf()) return;
	if(node){
		Renderer::setNodeGlData(ff.node,false/*set refPos only for the first time*/);
		Renderer::renderRawNode(ff.node);
		if(ff.node->rep) ff.node->rep->render(ff.node,&viewInfo);
		#ifdef MEMBRANE_DEBUG_ROT
			// show what Membrane thinks the orientation of nodes is - render those midway
			if(ff.currRot.size()==3){
				for(int i:{0,1,2}){
					shared_ptr<Node> n=make_shared<Node>();
					n->pos=ff.node->pos+.5*(ff.nodes[i]->pos-ff.node->pos);
					n->ori=(ff.currRot[i]*ff.node->ori.conjugate()).conjugate();
					Renderer::renderRawNode(n);
				}
			}
		#endif
	}

	// draw everything in in local coords now
	glPushMatrix();
		if(!Renderer::scaleOn){
			// without displacement scaling, local orientation is easy
			GLUtils::setLocalCoords(ff.node->pos,ff.node->ori);
		} else {
			/* otherwise compute scaled orientation such that
				* +x points to the same point on the triangle (in terms of angle)
				* +z is normal to the facet in the GL space
			*/
			Real phi0=atan2(ff.refPos[1],ff.refPos[0]);
			Vector3r C=ff.getGlCentroid();
			Matrix3r rot;
			rot.row(2)=ff.getGlNormal();
			rot.row(0)=(ff.getGlVertex(0)-C).normalized();
			rot.row(1)=rot.row(2).cross(rot.row(0));
			Quaternionr ori=AngleAxisr(phi0,Vector3r::UnitZ())*Quaternionr(rot);
			GLUtils::setLocalCoords(C,ori.conjugate());
		}

		if(refConf){
			glColor3v(refColor);
			glLineWidth(refWd);
			glBegin(GL_LINE_LOOP);
				for(int i:{0,1,2}) glVertex3v(Vector3r(ff.refPos[2*i],ff.refPos[2*i+1],0));
			glEnd();
		}
		if(uScale!=0){
			glLineWidth(uWd);
			for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),uScale*ff.uXy.segment<2>(2*i),uRange,uSplit,arrows?1:0,uWd);
		}
		if(relPhi!=0 && ff.hasBending()){
			glLineWidth(phiWd);
			for(int i:{0,1,2}) drawLocalDisplacement(ff.refPos.segment<2>(2*i),relPhi*viewInfo.sceneRadius*ff.phiXy.segment<2>(2*i),phiRange,phiSplit,arrows?2:0,phiWd
				#ifdef MEMBRANE_DEBUG_ROT
					, relPhi*viewInfo.sceneRadius*ff.drill[i] /* show the out-of-plane component */
				#endif
			);
		}
	glPopMatrix();
}