C++ Quaternionf::coeffs方法代码示例

// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void _psmove_orientation_fusion_imu_update(
	PSMoveOrientation *orientation_state,
	float delta_t,
	const Eigen::Vector3f &current_omega,
	const Eigen::Vector3f &current_g)
{
	// Current orientation from earth frame to sensor frame
	Eigen::Quaternionf SEq = orientation_state->quaternion;
	Eigen::Quaternionf SEq_new = SEq;

	// Compute the quaternion derivative measured by gyroscopes
	// Eqn 12) q_dot = 0.5*q*omega
	Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
	Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) *omega;

	if (current_g.isApprox(Eigen::Vector3f::Zero(), k_normal_epsilon))
	{
		// Get the direction of the gravitational fields in the identity pose		
		PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
		Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);

		// Eqn 15) Applied to the gravity vector
		// Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g|
		Eigen::Matrix<float, 3, 1> f_g;
		psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

		// Eqn 21) Applied to the gravity vector
		// Fill in the 4x3 objective function Jacobian matrix: J_gb(SEq)= [J_g]
		Eigen::Matrix<float, 4, 3> J_g;
		psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

		// Eqn 34) gradient_F= J_g(SEq)*f(SEq, Sa)
		// Compute the gradient of the objective function
		Eigen::Matrix<float, 4, 1> gradient_f = J_g * f_g;
		Eigen::Quaternionf SEqHatDot =
			Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

		// normalize the gradient
		psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);

		// Compute the estimated quaternion rate of change
		// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
		Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

		// Compute then integrate the estimated quaternion rate
		// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
		SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t);
	}
	else
	{
		SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_omega.coeffs()*delta_t);
	}

	// Make sure the net quaternion is a pure rotation quaternion
	SEq_new.normalize();

	// Save the new quaternion back into the orientation state
	orientation_state->quaternion = SEq_new;
}

static Eigen::Quaternionf 
angular_velocity_to_quaternion_derivative(
    const Eigen::Quaternionf &current_orientation,
    const Eigen::Vector3f &ang_vel)
{
    Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, ang_vel.x(), ang_vel.y(), ang_vel.z());
    Eigen::Quaternionf quaternion_derivative = Eigen::Quaternionf(current_orientation.coeffs() * 0.5f) *omega;

    return quaternion_derivative;
}

	bool update_map(rm_localization::UpdateMap::Request &req,
			rm_localization::UpdateMap::Response &res) {

		boost::mutex::scoped_lock lock(closest_keyframe_update_mutex);

		Eigen::Vector3f intrinsics;
		memcpy(intrinsics.data(), req.intrinsics.data(), 3 * sizeof(float));

		bool update_intrinsics = intrinsics[0] != 0.0f;

		if (update_intrinsics) {
			ROS_INFO("Updated camera intrinsics");
			this->intrinsics = intrinsics;
			ROS_INFO_STREAM("New intrinsics" << std::endl << this->intrinsics);
		}

		for (size_t i = 0; i < req.idx.size(); i++) {

			Eigen::Quaternionf orientation;
			Eigen::Vector3f position, intrinsics;

			memcpy(orientation.coeffs().data(),
					req.transform[i].unit_quaternion.data(), 4 * sizeof(float));
			memcpy(position.data(), req.transform[i].position.data(),
					3 * sizeof(float));

			Sophus::SE3f new_pos(orientation, position);

			if (req.idx[i] == closest_keyframe_idx) {
				//closest_keyframe_update_mutex.lock();

				camera_position = new_pos
						* keyframes[req.idx[i]]->get_pos().inverse()
						* camera_position;

				keyframes[req.idx[i]]->get_pos() = new_pos;

				//if (update_intrinsics) {
				//	keyframes[req.idx[i]]->get_intrinsics() = intrinsics;
				//}

				//closest_keyframe_update_mutex.unlock();

			} else {
				keyframes[req.idx[i]]->get_pos() = new_pos;

				//if (update_intrinsics) {
				//	keyframes[req.idx[i]]->get_intrinsics() = intrinsics;
				//}

			}
		}

		return true;
	}

bool
psmove_alignment_quaternion_between_vector_frames(
	const Eigen::Vector3f* from[2], const Eigen::Vector3f* to[2], const float tolerance, const Eigen::Quaternionf &initial_q,
	Eigen::Quaternionf &out_q)
{
	bool success = true;

	Eigen::Quaternionf previous_q = initial_q;
	Eigen::Quaternionf q = initial_q;
	
    //const float tolerance_squared = tolerance*tolerance; //TODO: This variable is unused, but it should be. Need to re-test with this added since the below test should be: error_squared > tolerance_squared
	const int k_max_iterations = 32;
	float previous_error_squared = k_real_max;
	float error_squared = k_real_max;
	float squared_error_delta = k_real_max;
	float gamma = 0.5f;
	bool backtracked = false;

	for (int iteration = 0; 
		iteration < k_max_iterations && // Haven't exceeded iteration limit
		error_squared > tolerance && // Aren't within tolerance of the destination
		squared_error_delta > k_normal_epsilon && // Haven't reached a minima
		gamma > k_normal_epsilon; // Haven't reduced our step size to zero
		iteration++)
	{
		// Fill in the 6x1 objective function matrix |f_0|
		//                                           |f_1|
		float error_squared0, error_squared1;

		Eigen::Matrix<float, 3, 1> f_0;
		psmove_alignment_compute_objective_vector(q, *from[0], *to[0], f_0, &error_squared0);

		Eigen::Matrix<float, 3, 1> f_1;
		psmove_alignment_compute_objective_vector(q, *from[1], *to[1], f_1, &error_squared1);

		Eigen::Matrix<float, 6, 1> f;
		f.block<3, 1>(0, 0) = f_0;
		f.block<3, 1>(3, 0) = f_1;

		error_squared = error_squared0 + error_squared1;

		// Make sure this new step hasn't made the error worse
		if (error_squared <= previous_error_squared)
		{
			// We won't have a valid error derivative if we had to back track
			squared_error_delta = !backtracked ? fabsf(error_squared - previous_error_squared) : squared_error_delta;
			backtracked = false;

			// This is a good step.
			// Remember it in case the next one makes things worse
			previous_error_squared = error_squared;
			previous_q = q;

			// Fill in the 4x6 objective function Jacobian matrix: [J_0|J_1]
			Eigen::Matrix<float, 4, 3> J_0;
			psmove_alignment_compute_objective_jacobian(q, *from[0], J_0);

			Eigen::Matrix<float, 4, 3> J_1;
			psmove_alignment_compute_objective_jacobian(q, *from[1], J_1);

			Eigen::Matrix<float, 4, 6> J;
			J.block<4, 3>(0, 0) = J_0; J.block<4, 3>(0, 3) = J_1;

			// Compute the gradient of the objective function
			Eigen::Matrix<float, 4, 1> gradient_f = J*f;
			Eigen::Quaternionf gradient_q =
				Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

			// The gradient points toward the maximum, so we subtract it off to head toward the minimum.
			// The step scale 'gamma' is just a guess.			
			q = Eigen::Quaternionf(q.coeffs() - gradient_q.coeffs()*gamma); //q-= gradient_q*gamma;
			q.normalize();
		}
		else
		{
			// The step made the error worse.
			// Return to the previous orientation and half our step size.
			q = previous_q;
			gamma /= 2.f;
			backtracked = true;
		}
	}

	if (error_squared > tolerance)
	{
		// Make sure we didn't fail to converge on the goal
		success = false;
	}

	out_q= q;

	return success;
}

// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void 
_psmove_orientation_fusion_madgwick_marg_update(
	PSMoveOrientation *orientation_state,
	float delta_t,
	const Eigen::Vector3f &current_omega,
	const Eigen::Vector3f &current_g,
	const Eigen::Vector3f &current_m)
{
	// If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update
	if (current_g.isZero(k_normal_epsilon) || current_m.isZero(k_normal_epsilon))
	{
		_psmove_orientation_fusion_imu_update(
			orientation_state,
			delta_t,
			current_omega,
			current_g);
		return;
	}

	PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;

	// Current orientation from earth frame to sensor frame
	Eigen::Quaternionf SEq = orientation_state->quaternion;

	// Get the direction of the magnetic fields in the identity pose.	
	// NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46)
	// but since we've already done the work in calibration to get this vector, let's just use it.
	// This also removes the last assumption in this function about what 
	// the orientation of the identity-pose is (handled by the sensor transform).
	PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
	Eigen::Vector3f k_identity_m_direction = Eigen::Vector3f(identity_m.x, identity_m.y, identity_m.z);

	// Get the direction of the gravitational fields in the identity pose
	PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
	Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);

	// Eqn 15) Applied to the gravity and magnetometer vectors
	// Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g|
	//                                                               |f_b|
	Eigen::Matrix<float, 3, 1> f_g;
	psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

	Eigen::Matrix<float, 3, 1> f_m;
	psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL);

	Eigen::Matrix<float, 6, 1> f_gb;
	f_gb.block<3, 1>(0, 0) = f_g;
	f_gb.block<3, 1>(3, 0) = f_m;

	// Eqn 21) Applied to the gravity and magnetometer vectors
	// Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b]
	Eigen::Matrix<float, 4, 3> J_g;
	psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

	Eigen::Matrix<float, 4, 3> J_m;
	psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m);

	Eigen::Matrix<float, 4, 6> J_gb;
	J_gb.block<4, 3>(0, 0) = J_g; J_gb.block<4, 3>(0, 3) = J_m;

	// Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm)
	// Compute the gradient of the objective function
	Eigen::Matrix<float, 4, 1> gradient_f = J_gb*f_gb;
	Eigen::Quaternionf SEqHatDot =
		Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

	// normalize the gradient to estimate direction of the gyroscope error
	psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);

	// Eqn 47) omega_err= 2*SEq*SEqHatDot
	// compute angular estimated direction of the gyroscope error
	Eigen::Quaternionf omega_err = Eigen::Quaternionf(SEq.coeffs()*2.f) * SEqHatDot;

	// Eqn 48) net_omega_bias+= zeta*omega_err
	// Compute the net accumulated gyroscope bias
	marg_state->omega_bias = Eigen::Quaternionf(marg_state->omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_t);
	marg_state->omega_bias.w() = 0.f; // no bias should accumulate on the w-component

	// Eqn 49) omega_corrected = omega - net_omega_bias
	Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
	Eigen::Quaternionf corrected_omega = Eigen::Quaternionf(omega.coeffs() - marg_state->omega_bias.coeffs());

	// Compute the rate of change of the orientation purely from the gyroscope
	// Eqn 12) q_dot = 0.5*q*omega
	Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) * corrected_omega;

	// Compute the estimated quaternion rate of change
	// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
	Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

	// Compute then integrate the estimated quaternion rate
	// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
	Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t);

	// Make sure the net quaternion is a pure rotation quaternion
	SEq_new.normalize();

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

// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void 
orientation_fusion_madgwick_marg_update(
    const float delta_time,
    const OrientationFilterSpace *filter_space,
    const OrientationFilterPacket *filter_packet,
    OrientationSensorFusionState *fusion_state)
{
    const Eigen::Vector3f &current_omega= filter_packet->gyroscope;
    const Eigen::Vector3f &current_g= filter_packet->normalized_accelerometer;
    const Eigen::Vector3f &current_m= filter_packet->normalized_magnetometer;

    // If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update
    if (current_g.isZero(k_normal_epsilon) || current_m.isZero(k_normal_epsilon))
    {
        orientation_fusion_imu_update(
            delta_time,
            filter_space,
            filter_packet,
            fusion_state);
        return;
    }

    MadgwickMARGState *marg_state = &fusion_state->fusion_state.madgwick_marg_state;

    // Current orientation from earth frame to sensor frame
    const Eigen::Quaternionf SEq = fusion_state->orientation;

    // Get the direction of the magnetic fields in the identity pose.	
    // NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46)
    // but since we've already done the work in calibration to get this vector, let's just use it.
    // This also removes the last assumption in this function about what 
    // the orientation of the identity-pose is (handled by the sensor transform).
    Eigen::Vector3f k_identity_m_direction = filter_space->getMagnetometerCalibrationDirection();

    // Get the direction of the gravitational fields in the identity pose
    Eigen::Vector3f k_identity_g_direction = filter_space->getGravityCalibrationDirection();

    // Eqn 15) Applied to the gravity and magnetometer vectors
    // Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g|
    //                                                               |f_b|
    Eigen::Matrix<float, 3, 1> f_g;
    eigen_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

    Eigen::Matrix<float, 3, 1> f_m;
    eigen_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL);

    Eigen::Matrix<float, 6, 1> f_gb;
    f_gb.block<3, 1>(0, 0) = f_g;
    f_gb.block<3, 1>(3, 0) = f_m;

    // Eqn 21) Applied to the gravity and magnetometer vectors
    // Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b]
    Eigen::Matrix<float, 4, 3> J_g;
    eigen_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

    Eigen::Matrix<float, 4, 3> J_m;
    eigen_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m);

    Eigen::Matrix<float, 4, 6> J_gb;
    J_gb.block<4, 3>(0, 0) = J_g; J_gb.block<4, 3>(0, 3) = J_m;

    // Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm)
    // Compute the gradient of the objective function
    Eigen::Matrix<float, 4, 1> gradient_f = J_gb*f_gb;
    Eigen::Quaternionf SEqHatDot =
        Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

    // normalize the gradient to estimate direction of the gyroscope error
    eigen_quaternion_normalize_with_default(SEqHatDot, *k_eigen_quaternion_zero);

    // Eqn 47) omega_err= 2*SEq*SEqHatDot
    // compute angular estimated direction of the gyroscope error
    Eigen::Quaternionf omega_err = Eigen::Quaternionf(SEq.coeffs()*2.f) * SEqHatDot;

    // Eqn 48) net_omega_bias+= zeta*omega_err
    // Compute the net accumulated gyroscope bias
    Eigen::Quaternionf omega_bias= marg_state->omega_bias;
    omega_bias = Eigen::Quaternionf(omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_time);
    omega_bias.w() = 0.f; // no bias should accumulate on the w-component
    marg_state->omega_bias= omega_bias;

    // Eqn 49) omega_corrected = omega - net_omega_bias
    Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
    Eigen::Quaternionf corrected_omega = Eigen::Quaternionf(omega.coeffs() - omega_bias.coeffs());

    // Compute the rate of change of the orientation purely from the gyroscope
    // Eqn 12) q_dot = 0.5*q*omega
    Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) * corrected_omega;

    // Compute the estimated quaternion rate of change
    // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
    Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

    // Compute then integrate the estimated quaternion rate
    // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
    Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_time);

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

void OrientationFilter::update(
    const float delta_time, 
    const OrientationSensorPacket &sensorPacket)
{
    OrientationFilterPacket filterPacket;
    m_FilterSpace.convertSensorPacketToFilterPacket(sensorPacket, filterPacket);

    const Eigen::Quaternionf orientation_backup= m_FusionState->orientation;
    const Eigen::Quaternionf first_derivative_backup = m_FusionState->orientation_first_derivative;
    const Eigen::Quaternionf second_derivative_backup = m_FusionState->orientation_second_derivative;

    switch(m_FusionState->fusion_type)
    {
    case FusionTypeNone:
        break;
    case FusionTypePassThru:
        {
            const Eigen::Quaternionf &new_orientation= filterPacket.orientation;
            const Eigen::Quaternionf new_first_derivative=
                Eigen::Quaternionf(
                    (new_orientation.coeffs() - m_FusionState->orientation.coeffs())
                    / delta_time);
            const Eigen::Quaternionf new_second_derivative =
                Eigen::Quaternionf(
                    (new_first_derivative.coeffs() - m_FusionState->orientation_first_derivative.coeffs())
                    / delta_time);

            m_FusionState->orientation = new_orientation;
            m_FusionState->orientation_first_derivative = new_first_derivative;
            m_FusionState->orientation_second_derivative = new_second_derivative;
        }
        break;
    case FusionTypeMadgwickIMU:
        orientation_fusion_imu_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState);
        break;
    case FusionTypeMadgwickMARG:
        orientation_fusion_madgwick_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState);
        break;
    case FusionTypeComplementaryMARG:
        orientation_fusion_complementary_marg_update(delta_time, &m_FilterSpace, &filterPacket, m_FusionState);
        break;
    }

    if (!eigen_quaternion_is_valid(m_FusionState->orientation)) 
    {
        SERVER_LOG_WARNING("OrientationFilter") << "Orientation is NaN!";
        m_FusionState->orientation = orientation_backup;
    }

    if (!eigen_quaternion_is_valid(m_FusionState->orientation_first_derivative))
    {
        SERVER_LOG_WARNING("OrientationFilter") << "Orientation first derivative is NaN!";
        m_FusionState->orientation_first_derivative = first_derivative_backup;
    }

    if (!eigen_quaternion_is_valid(m_FusionState->orientation_second_derivative))
    {
        SERVER_LOG_WARNING("OrientationFilter") << "Orientation second derivative is NaN!";
        m_FusionState->orientation_second_derivative = second_derivative_backup;
    }
}

// -- OrientationFilterComplementaryOpticalARG --
void OrientationFilterComplementaryOpticalARG::update(const float delta_time, const PoseFilterPacket &packet)
{
    if (packet.tracking_projection_area_px_sqr <= k_real_epsilon)
    {
        OrientationFilterMadgwickARG::update(delta_time, packet);
		return;
    }

    const Eigen::Vector3f &current_omega= packet.imu_gyroscope_rad_per_sec;

    Eigen::Vector3f current_g= packet.imu_accelerometer_g_units;
    eigen_vector3f_normalize_with_default(current_g, Eigen::Vector3f::Zero());

    // Current orientation from earth frame to sensor frame
    const Eigen::Quaternionf SEq = m_state->orientation;
    Eigen::Quaternionf SEq_new = SEq;

    // Compute the quaternion derivative measured by gyroscopes
    // Eqn 12) q_dot = 0.5*q*omega
    Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
    Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) *omega;

    if (!current_g.isApprox(Eigen::Vector3f::Zero(), k_normal_epsilon))
    {
        // Get the direction of the gravitational fields in the identity pose		
        Eigen::Vector3f k_identity_g_direction = m_constants.gravity_calibration_direction;

        // Eqn 15) Applied to the gravity vector
        // Fill in the 3x1 objective function matrix f(SEq, Sa) =|f_g|
        Eigen::Matrix<float, 3, 1> f_g;
        eigen_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

        // Eqn 21) Applied to the gravity vector
        // Fill in the 4x3 objective function Jacobian matrix: J_gb(SEq)= [J_g]
        Eigen::Matrix<float, 4, 3> J_g;
        eigen_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

        // Eqn 34) gradient_F= J_g(SEq)*f(SEq, Sa)
        // Compute the gradient of the objective function
        Eigen::Matrix<float, 4, 1> gradient_f = J_g * f_g;
        Eigen::Quaternionf SEqHatDot =
            Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

        // normalize the gradient
        eigen_quaternion_normalize_with_default(SEqHatDot, *k_eigen_quaternion_zero);

        // Compute the estimated quaternion rate of change
        // Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
        const float beta= sqrtf(3.0f / 4.0f) * fmaxf(fmaxf(m_constants.gyro_variance.x(), m_constants.gyro_variance.y()), m_constants.gyro_variance.z());
        Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

        // Compute then integrate the estimated quaternion rate
        // Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
        SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_time);
    }
    else
    {
        SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_omega.coeffs()*delta_time);
    }

    // Make sure the net quaternion is a pure rotation quaternion
    SEq_new.normalize();

    // Save the new quaternion and first derivative back into the orientation state
    // Derive the second derivative
    {
        // The final rotation is a blend between the integrated orientation and absolute optical orientation
		const float max_variance_fraction =
			safe_divide_with_default(
				m_constants.orientation_variance_curve.evaluate(packet.tracking_projection_area_px_sqr),
				m_constants.orientation_variance_curve.MaxValue,
				1.f);
        float optical_weight= 
			lerp_clampf(1.f - max_variance_fraction, 0, k_max_optical_orientation_weight);
        
        static float g_weight_override= -1.f;
        if (g_weight_override >= 0.f)
        {
            optical_weight= g_weight_override;
        }

		const Eigen::EulerAnglesf optical_euler_angles = eigen_quaternionf_to_euler_angles(packet.optical_orientation);
		const Eigen::EulerAnglesf SEeuler_new= eigen_quaternionf_to_euler_angles(packet.optical_orientation);

		// Blend in the yaw from the optical orientation
		const float blended_heading_radians= 
			wrap_lerpf(
				SEeuler_new.get_heading_radians(), 
				optical_euler_angles.get_heading_radians(), 
				optical_weight, 
				-k_real_pi, k_real_pi);
		const Eigen::EulerAnglesf new_euler_angles(
			SEeuler_new.get_bank_radians(), blended_heading_radians, SEeuler_new.get_attitude_radians());
		const Eigen::Quaternionf new_orientation =
			eigen_euler_angles_to_quaternionf(new_euler_angles);

        const Eigen::Vector3f &new_angular_velocity= current_omega;
        const Eigen::Vector3f new_angular_acceleration = (current_omega - m_state->angular_velocity) / delta_time;

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