C++ Quaternion函数代码示例

Quaternion Deserializer::ReadQuaternion()
{
    float data[4];
    Read(data, sizeof data);
    return Quaternion(data);
}

int main(int argc, char* argv[]) {
	if (argc == 2) {
		omp_set_num_threads(atoi(argv[1]));

	} else {

		omp_set_num_threads(1);
	}

	//=========================================================================================================
	ChSystemGPU * system_gpu = new ChSystemGPU;
	ChCollisionSystemGPU *mcollisionengine = new ChCollisionSystemGPU();
	system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
	//=========================================================================================================
	system_gpu->SetMaxiter(max_iter);
	system_gpu->SetIterLCPmaxItersSpeed(max_iter);
	((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIteration(max_iter);
	system_gpu->SetTol(1e-3);
	system_gpu->SetTolSpeeds(1e-3);
	((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(1e-3);
	((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance(0, 0, 0);
	((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(.6);
	((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(ACCELERATED_PROJECTED_GRADIENT_DESCENT);
	((ChCollisionSystemGPU *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius.x * .05);
	mcollisionengine->setBinsPerAxis(R3(num_per_dir.x * 2, num_per_dir.y * 2, num_per_dir.z * 2));
	mcollisionengine->setBodyPerBin(100, 50);
	system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
	system_gpu->SetStep(timestep);
	//=========================================================================================================
	cout << num_per_dir.x << " " << num_per_dir.y << " " << num_per_dir.z << " " << num_per_dir.x * num_per_dir.y * num_per_dir.z << endl;
	if (particle_radius.x == particle_radius.y == particle_radius.z) {
		addPerturbedLayer(R3(0, -5 + particle_radius.y + container_thickness, 0), SPHERE, particle_radius, num_per_dir, R3(.01, .01, .01), 4, .1, system_gpu);

	} else {
		addPerturbedLayer(R3(0, -5 + particle_radius.y + container_thickness, 0), ELLIPSOID, particle_radius, num_per_dir, R3(.01, .01, .01), 4, .1, system_gpu);
	}
	//=========================================================================================================

	ChSharedBodyPtr L = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
	ChSharedBodyPtr R = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
	ChSharedBodyPtr F = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
	ChSharedBodyPtr B = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
	ChSharedBodyPtr Bottom = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));

	InitObject(L, 100000, Vector(-container_size.x + container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true,
			-20, -20);
	InitObject(R, 100000, Vector(container_size.x - container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true, -20,
			-20);
	InitObject(F, 100000, Vector(0, container_height - container_thickness, -container_size.z + container_thickness), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true,
			-20, -20);
	InitObject(B, 100000, Vector(0, container_height - container_thickness, container_size.z - container_thickness), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true, -20,
			-20);
	InitObject(Bottom, 100000, Vector(0, container_height - container_size.y, 0), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true, -20, -20);

	AddCollisionGeometry(L, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
	AddCollisionGeometry(R, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
	AddCollisionGeometry(F, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
	AddCollisionGeometry(B, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
	AddCollisionGeometry(Bottom, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));

	FinalizeObject(L, (ChSystemGPU *) system_gpu);
	FinalizeObject(R, (ChSystemGPU *) system_gpu);
	FinalizeObject(F, (ChSystemGPU *) system_gpu);
	FinalizeObject(B, (ChSystemGPU *) system_gpu);
	FinalizeObject(Bottom, (ChSystemGPU *) system_gpu);

	//=========================================================================================================
	//////Rendering specific stuff:
	ChOpenGLManager * window_manager = new ChOpenGLManager();
	ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
	openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
	openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
	openGLView.render_camera->camera_scale = .1;
	openGLView.SetCustomCallback(RunTimeStep);
	openGLView.StartSpinning(window_manager);
	window_manager->CallGlutMainLoop();
	//=========================================================================================================

	for (int i = 0; i < num_steps; i++) {
		system_gpu->DoStepDynamics(timestep);
		double TIME = system_gpu->GetChTime();
		double STEP = system_gpu->GetTimerStep();
		double BROD = system_gpu->GetTimerCollisionBroad();
		double NARR = system_gpu->GetTimerCollisionNarrow();
		double LCP = system_gpu->GetTimerLcp();
		double UPDT = system_gpu->GetTimerUpdate();
		int BODS = system_gpu->GetNbodies();
		int CNTC = system_gpu->GetNcontacts();
		int REQ_ITS = ((ChLcpSolverGPU*) (system_gpu->GetLcpSolverSpeed()))->GetTotalIterations();

		printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d\n", TIME, STEP, BROD, NARR, LCP, UPDT, BODS, CNTC, REQ_ITS);
		if (i % 1000 == 0) {
			cout << "SAVED STATE" << endl;
			DumpObjects(system_gpu, "diagonal_impact_settled.txt", "\t");
		}
		RunTimeStep(system_gpu, i);
	}

	DumpObjects(system_gpu, "diagonal_impact_settled.txt", "\t");

//.........这里部分代码省略.........

// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
{
    uint32_t tnow = AP_HAL::millis();

    // initialize vertical speeds and acceleration
    pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
    pos_control.set_max_accel_z(g.pilot_accel_z);

    if (!motors.armed()) {
        motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
        // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
        attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
        last_pilot_heading = ahrs.yaw_sensor;
        return;
    }

    motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

    // get pilot desired lean angles
    float target_roll, target_pitch;

    // Check if set_attitude_target_no_gps is valid
    if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
        float target_yaw;
        Quaternion(
            set_attitude_target_no_gps.packet.q
        ).to_euler(
            target_roll,
            target_pitch,
            target_yaw
        );
        target_roll = degrees(target_roll);
        target_pitch = degrees(target_pitch);
        target_yaw = degrees(target_yaw);

        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
        return;
    }

    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());

    // get pilot's desired yaw rate
    float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());

    // call attitude controller
    if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
        last_pilot_heading = ahrs.yaw_sensor;
        last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading

    } else { // hold current heading

        // this check is required to prevent bounce back after very fast yaw maneuvers
        // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
        if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
            target_yaw_rate = 0; // Stop rotation on yaw axis

            // call attitude controller with target yaw rate = 0 to decelerate on yaw axis
            attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
            last_pilot_heading = ahrs.yaw_sensor; // update heading to hold

        } else { // call attitude controller holding absolute absolute bearing
            attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
        }
    }

    if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%
        // output pilot's throttle
        attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
        // reset z targets to current values
        pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
    } else { // hold z

        if (ap.at_bottom) {
            pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
            pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
        } else if (rangefinder_alt_ok()) {
            // if rangefinder is ok, use surface tracking
            float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt);
            pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
        }
        pos_control.update_z_controller();
    }

    motors.set_forward(channel_forward->norm_input());
    motors.set_lateral(channel_lateral->norm_input());
}

	//-----------------------------------------------------------------------
	void Extruder::_extrudeCapImpl(TriangleBuffer& buffer) const
	{
		std::vector<int> indexBuffer;
		PointList pointList;

		buffer.rebaseOffset();

		Triangulator t;
		if (mShapeToExtrude)
			t.setShapeToTriangulate(mShapeToExtrude);
		else
			t.setMultiShapeToTriangulate(mMultiShapeToExtrude);
		t.triangulate(indexBuffer, pointList);
		buffer.estimateIndexCount(2*indexBuffer.size());
		buffer.estimateVertexCount(2*pointList.size());


		//begin cap
		buffer.rebaseOffset();
		Quaternion qBegin = Utils::_computeQuaternion(mExtrusionPath->getDirectionAfter(0));
		if (mRotationTrack)
		{
			Real angle = mRotationTrack->getFirstValue();
			qBegin = qBegin*Quaternion((Radian)angle, Vector3::UNIT_Z);
		}	
		Real scaleBegin=1.;
		if (mScaleTrack)
			scaleBegin = mScaleTrack->getFirstValue();
		for (size_t j =0;j<pointList.size();j++)
		{
			Vector2 vp2 = pointList[j];
			Vector3 vp(vp2.x, vp2.y, 0);
			Vector3 normal = -Vector3::UNIT_Z;				

			Vector3 newPoint = mExtrusionPath->getPoint(0)+qBegin*(scaleBegin*vp);
			addPoint(buffer, newPoint,
				qBegin*normal,
				vp2);
		}

		for (size_t i=0;i<indexBuffer.size()/3;i++)
		{				
			buffer.index(indexBuffer[i*3]);
			buffer.index(indexBuffer[i*3+2]);
			buffer.index(indexBuffer[i*3+1]);
		}

		// end cap
		buffer.rebaseOffset();
		Quaternion qEnd = Utils::_computeQuaternion(mExtrusionPath->getDirectionBefore(mExtrusionPath->getSegCount()));
		if (mRotationTrack)
		{
			Real angle = mRotationTrack->getLastValue();
			qEnd = qEnd*Quaternion((Radian)angle, Vector3::UNIT_Z);
		}			
		Real scaleEnd=1.;
		if (mScaleTrack)
			scaleEnd = mScaleTrack->getLastValue();

		for (size_t j =0;j<pointList.size();j++)
		{
			Vector2 vp2 = pointList[j];
			Vector3 vp(vp2.x, vp2.y, 0);
			Vector3 normal = Vector3::UNIT_Z;				

			Vector3 newPoint = mExtrusionPath->getPoint(mExtrusionPath->getSegCount())+qEnd*(scaleEnd*vp);
			addPoint(buffer, newPoint,
				qEnd*normal,
				vp2);
		}

		for (size_t i=0;i<indexBuffer.size()/3;i++)
		{				
			buffer.index(indexBuffer[i*3]);
			buffer.index(indexBuffer[i*3+1]);
			buffer.index(indexBuffer[i*3+2]);
		}

	}

 Quaternion operator*( real_t s ) const {
     return Quaternion( w * s, x * s, y * s, z * s );
 }

	Quaternion Quaternion::RotationBetween(Vector3f a, Vector3f b)
	{
		return Quaternion();
	}

void setupAnimation(void)
{
    //Color Keyframe Sequence
    ColorKeyframes = KeyframeColorsSequence3f::create();
    ColorKeyframes->addKeyframe(Color4f(1.0f,0.0f,0.0f,1.0f),0.0f);
    ColorKeyframes->addKeyframe(Color4f(0.0f,1.0f,0.0f,1.0f),2.0f);
    ColorKeyframes->addKeyframe(Color4f(0.0f,0.0f,1.0f,1.0f),4.0f);
    ColorKeyframes->addKeyframe(Color4f(1.0f,0.0f,0.0f,1.0f),6.0f);

	//Vector Keyframe Sequence
    VectorKeyframes = KeyframeVectorsSequence3f::create();
    VectorKeyframes->addKeyframe(Vec3f(0.0f,0.0f,0.0f),0.0f);
    VectorKeyframes->addKeyframe(Vec3f(0.0f,1.0f,0.0f),1.0f);
    VectorKeyframes->addKeyframe(Vec3f(1.0f,1.0f,0.0f),2.0f);
    VectorKeyframes->addKeyframe(Vec3f(1.0f,0.0f,0.0f),3.0f);
    VectorKeyframes->addKeyframe(Vec3f(0.0f,0.0f,0.0f),4.0f);
    
	//Rotation Keyframe Sequence
    RotationKeyframes = KeyframeRotationsSequenceQuat::create();
    RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0),0.0f);
    RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.5),1.0f);
    RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.0),2.0f);
    RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.5),3.0f);
    RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0),4.0f);

	//Transformation Keyframe Sequence
    TransformationKeyframes = KeyframeTransformationsSequence44f::create();
	Matrix TempMat;
	TempMat.setTransform(Vec3f(0.0f,0.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0));
    TransformationKeyframes->addKeyframe(TempMat,1.0f);
	TempMat.setTransform(Vec3f(0.0f,1.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.5));
    TransformationKeyframes->addKeyframe(TempMat,2.0f);
	TempMat.setTransform(Vec3f(1.0f,1.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.0));
    TransformationKeyframes->addKeyframe(TempMat,3.0f);
	TempMat.setTransform(Vec3f(1.0f,0.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.5));
    TransformationKeyframes->addKeyframe(TempMat,4.0f);
	TempMat.setTransform(Vec3f(0.0f,0.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0));
    TransformationKeyframes->addKeyframe(TempMat,5.0f);
    
    //Animator
    TheAnimator = KeyframeAnimator::create();
    beginEditCP(TheAnimator, KeyframeAnimator::KeyframeSequenceFieldMask);
        TheAnimator->setKeyframeSequence(TransformationKeyframes);
    endEditCP(TheAnimator, KeyframeAnimator::KeyframeSequenceFieldMask);
    
    //Animation
    TheAnimation = FieldAnimation::create();
    beginEditCP(TheAnimation);
        TheAnimation->setAnimator(TheAnimator);
        TheAnimation->setInterpolationType(LINEAR_INTERPOLATION);
        TheAnimation->setCycling(-1);
    endEditCP(TheAnimation);
	TheAnimation->setAnimatedField(getFieldContainer("Transform",std::string("TorusNodeTransformationCore")), std::string("matrix"));

    //Animation Listener
    TheAnimation->addAnimationListener(&TheAnimationListener);

    //Animation Advancer
    TheAnimationAdvancer = ElapsedTimeAnimationAdvancer::create();
    beginEditCP(TheAnimationAdvancer);
    ElapsedTimeAnimationAdvancer::Ptr::dcast(TheAnimationAdvancer)->setStartTime( 0.0 );
    beginEditCP(TheAnimationAdvancer);
}

TransformObject::TransformObject(void)
    : pos(Vector3f()), scale(Vector3f(1.0f, 1.0f, 1.0f))
{
    SetRotation(Quaternion());
}

	Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
	{
		return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
	}

void Physics::step( real_t dt )
{
    // TODO step the world forward by dt. Need to detect collisions, apply
    // forces, and integrate positions and orientations.
    //
    // Note: put RK4 here, not in any of the physics bodies
    //
    // Must use the functions that you implemented
    //
    // Note, when you change the position/orientation of a physics object,
    // change the position/orientation of the graphical object that represents
    // it
	Derivative initial;
	Derivative k1, k2, k3, k4;
	Vector3 sumAngles;
	for(unsigned int i = 0; i < spheres.size(); i++){
		initial.velocity = spheres[i]->velocity;
		initial.angular_velocity = spheres[i]->angular_velocity;
		initial.position = spheres[i]->position;
		initial.orientation = spheres[i]->orientation;
		initial.angle = Vector3(0,0,0);
		resetForce(*spheres[i]);
		
		/****************************RK4**************************************/
		/* k1 */
		setPosition(initial, k1, initial, 0);
		updateState(*spheres[i], k1);
		spheres[i]->apply_force(gravity, Vector3(0,0,0));
		for(unsigned int j = 0; j < springs.size(); j++){
			if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
					springs[j]->step(dt);
		}
		Evaluate(k1, *spheres[i], dt);
		resetForce(*spheres[i]);
		

		/* k2 */
		setPosition(k1, k2, initial, dt/2);
		updateState(*spheres[i], k2);
		spheres[i]->apply_force(gravity, Vector3(0,0,0));
		for(unsigned int j = 0; j < springs.size(); j++){
			if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
					springs[j]->step(dt/2);
		}
		Evaluate(k2, *spheres[i], dt);
		resetForce(*spheres[i]);
		

		/* k3 */
		setPosition(k2, k3, initial, dt/2);
		updateState(*spheres[i], k3);
		spheres[i]->apply_force(gravity, Vector3(0,0,0));
		for(unsigned int j = 0; j < springs.size(); j++){
			if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
					springs[j]->step(dt/2);
		}
		Evaluate(k3, *spheres[i], dt);
		resetForce(*spheres[i]);
		
		/* k4 */
		setPosition(k3, k4, initial, dt);
		updateState(*spheres[i], k4);
		spheres[i]->apply_force(gravity, Vector3(0,0,0));
		for(unsigned int j = 0; j < springs.size(); j++){
			if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
					springs[j]->step(dt);
		}
		Evaluate(k4, *spheres[i], dt);
		resetForce(*spheres[i]);
	

		/***************udpate position and orientation after RK4***************/
		spheres[i]->position = initial.position + 1.0 / 6.0 * (k1.position + 2.0 * (k2.position + k3.position) + k4.position);
		spheres[i]->velocity = initial.velocity + 1.0 / 6.0 * (k1.velocity + 2.0 * (k2.velocity + k3.velocity) + k4.velocity);
		spheres[i]->angular_velocity = initial.angular_velocity + 1.0 / 6.0 * (k1.angular_velocity + 2.0 * (k2.angular_velocity + k3.angular_velocity) + k4.angular_velocity);

		sumAngles = 1.0 / 6.0 * (k1.angle + 2.0 * (k2.angle + k3.angle) + k4.angle);
		if(sumAngles != Vector3(0,0,0)){
			spheres[i]->orientation = initial.orientation * Quaternion(normalize(sumAngles), length(sumAngles));	
		}
		else{
			spheres[i]->orientation = initial.orientation;
		}

		spheres[i]->sphere->position = spheres[i]->position;
		spheres[i]->sphere->orientation = spheres[i]->orientation;

		
		/*******collision detection************/
		for(unsigned int j = 0; j < spheres.size(); j++){
			if(spheres[i]->id != spheres[j]->id){
				collides(*spheres[i], *spheres[j], collision_damping);
			}
		}

		for(unsigned int j = 0; j < triangles.size(); j++){
			collides(*spheres[i], *triangles[j], collision_damping);
		}

		for(unsigned int j = 0; j < planes.size(); j++){
//.........这里部分代码省略.........

void TransformObject::SetRotation(Vector3f eulerAngleAmounts)
{
    rot = Quaternion(eulerAngleAmounts);
    CalculateNewDirVectors();
}

//-----------------------------------------------------------------------
void Node::setOrientation( Real w, Real x, Real y, Real z)
{
    setOrientation(Quaternion(w, x, y, z));
}

void dtRotate(DtScalar x, DtScalar y, DtScalar z, DtScalar w) {
  if (currentObject) currentObject->rotate(Quaternion(x, y, z, w));
}

Quaternion Quaternion::Conjugate() {
  return Quaternion(s, -vx, -vy, -vz);
}

bool Ekf::initialiseFilter(void)
{
	// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter

	// Sum the IMU delta angle measurements
	imuSample imu_init = _imu_buffer.get_newest();
	_delVel_sum += imu_init.delta_vel;

	// Sum the magnetometer measurements
	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
		if (_mag_counter == 0) {
			_mag_filt_state = _mag_sample_delayed.mag;
		}

		_mag_counter ++;
		_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
	}

	// set the default height source from the adjustable parameter
	if (_hgt_counter == 0) {
		_primary_hgt_source = _params.vdist_sensor_type;
	}

	if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
			if (_hgt_counter == 0) {
				_control_status.flags.baro_hgt = false;
				_control_status.flags.gps_hgt = false;
				_control_status.flags.rng_hgt = true;
				_hgt_filt_state = _range_sample_delayed.rng;
			}

			_hgt_counter ++;
			_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
		}

	} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
		if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
			if (_hgt_counter == 0) {
				_control_status.flags.baro_hgt = true;
				_control_status.flags.gps_hgt = false;
				_control_status.flags.rng_hgt = false;
				_hgt_filt_state = _baro_sample_delayed.hgt;
			}

			_hgt_counter ++;
			_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
		}

	} else {
		return false;
	}

	// check to see if we have enough measurements and return false if not
	if (_hgt_counter <= 10 || _mag_counter <= 10) {
		return false;

	} else {
		// reset variables that are shared with post alignment GPS checks
		_gps_drift_velD = 0.0f;
		_gps_alt_ref = 0.0f;

		// Zero all of the states
		_state.ang_error.setZero();
		_state.vel.setZero();
		_state.pos.setZero();
		_state.gyro_bias.setZero();
		_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
		_state.accel_z_bias = 0.0f;
		_state.mag_I.setZero();
		_state.mag_B.setZero();
		_state.wind_vel.setZero();

		// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
		float pitch = 0.0f;
		float roll = 0.0f;

		if (_delVel_sum.norm() > 0.001f) {
			_delVel_sum.normalize();
			pitch = asinf(_delVel_sum(0));
			roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));

		} else {
			return false;
		}

		// calculate initial tilt alignment
		matrix::Euler<float> euler_init(roll, pitch, 0.0f);
		_state.quat_nominal = Quaternion(euler_init);
		_output_new.quat_nominal = _state.quat_nominal;

		// initialise the filtered alignment error estimate to a larger starting value
		_tilt_err_length_filt = 1.0f;

		// calculate the averaged magnetometer reading
		Vector3f mag_init = _mag_filt_state;

		// calculate the initial magnetic field and yaw alignment
		resetMagHeading(mag_init);

//.........这里部分代码省略.........