传给PX4的SLAM位姿需要满足什么条件，才能使得PX4基于SLAM位姿实现定点悬停。（草稿版）

传给PX4的SLAM位姿需要满足什么样的条件，才能使得PX4基于SLAM位姿实现定点悬停。

保证xyz和四元数是同一机体系在同一世界系下的位姿，如果xyz对应的世界系和四元数对应的世界系不一致，就会出现问题。

偏航可以认为是机体系x轴在世界系水平面的投影和世界系x轴的夹角，所以你这个夹角是需要和世界系x轴对应的，不能说我偏航是一个角度，但是xyz对应的世界系x轴又是另一个方向。

要弄清楚这个，涉及到PX4中两个最为关键的模块，姿态估计和控制。

1 要想知道如何可以让PX4无人机定住，我们首先要了解PX4的四环串级控制环。

图中字符和含义

1. Inertial Frame：惯性坐标系，通常是一个固定在地球上的坐标系，用于描述飞行器在空间中的绝对位置、速度和加速度。
2. Body Frame：机体坐标系，与飞行器固连的坐标系，随着飞行器一起运动和旋转，用于描述飞行器自身的姿态（角度和角速度）。
3. Xsp

• ：位置设定值（Position Setpoint），表示期望飞行器到达的三维空间位置（在惯性坐标系下）。
• Vsp
• ：速度设定值（Velocity Setpoint），对应于期望的飞行器的线速度（在惯性坐标系下）。
• Asp
• ：加速度设定值（Acceleration Setpoint），是期望飞行器达到的线加速度（在惯性坐标系下）。
• ψsp
• ：偏航角设定值（Yaw Setpoint），指定飞行器期望的偏航角度。
• P
• ：比例控制器（Proportional Controller），根据误差的当前值来产生控制输出。
• PID
• ：比例-积分-微分控制器（Proportional-Integral-Derivative Controller），结合了比例、积分和微分三种控制作用，能更有效地纠正系统误差。
• qsp
• ：姿态四元数设定值（Attitude Quaternion Setpoint），描述期望的飞行器的姿态（在机体坐标系下用四元数表示）。
• Ωsp
• ：角速度设定值（Angular Rate Setpoint），表示期望飞行器旋转的角速度（在机体坐标系下）。
• δAsp
• 、δEsp、δRsp、δTsp
• ：分别是副翼（Aileron）、升降舵（Elevator）、方向舵（Rudder）和油门（Throttle）的控制量设定值，用于后续驱动电机或舵机等执行机构。
• Tsp

1. ：电机推力设定值（Thrust Setpoint），是经过混控器（Mixer）转换后得到的各个电机期望产生的推力值。
2. Mixer：混控器，根据飞行器的控制配置（如多旋翼的不同机架结构），将期望的姿态和推力指令转换为各个电机或舵机的具体控制量。

简单介绍一下PX4的四环串级控制
四环控制由外到内依次为：

1. 位置环（Position Loop）

   • 输入：期望位置（X/Y/Z）、当前位置（来自GPS或视觉定位）
   • 输出：期望速度（速度指令）

2. 速度环（Velocity Loop）

   • 输入：期望速度（来自位置环）、当前速度（来自惯性传感器或位置差分）
   • 输出：期望姿态角（Roll/Pitch）和推力（Thrust）

3. 姿态环（Attitude Loop）

   • 输入：期望姿态角（来自速度环）、当前姿态角（来自IMU的欧拉角或四元数）
   • 输出：期望角速率（Roll Rate/Pitch Rate/Yaw Rate）

4. 角速率环（Rate Loop）

   • 输入：期望角速率（来自姿态环）、当前角速率（来自IMU的陀螺仪数据）
   • 输出：电机PWM控制信号

位置环输出期望速度，速度环输出期望加速度，也就是期望推力 结合偏航得到期望姿态+推力，所以高飞为什么发期望姿态+推力，也可以从这里得到，去除位置环速度环，剩下的输入就是期望姿态+推力

位置环和速度环不用考虑偏航可以这么理解，我偏航不管怎么转，都不影响无人机在世界系下的xyz对吧，就是这样。所以我约束它的xyz时候 不需要约束它的偏航

在角度环，输入为NED坐标系下四元数表示的当前姿态q和期望姿态qd，输出为机体坐标系下三轴的角速度。
开始是在NED下算期望姿态四元数和当前姿态四元数误差，然后把这个误差转到机体系下，这样再基于机体系下的误差PID后得到的角速度自然也是机体系下的了。

https://docs.px4.io/main/zh/modules/modules_controller.html
 

https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf
 

https://blog.csdn.net/hkzuibang/article/details/136947829
 

https://zhuanlan.zhihu.com/p/81659459
姿态控制器在得到期望四元数之后并不是马上进行角速率期望的计算，而是先进行倾转分离的操作。

那什么是倾转分离？倾转其实是倾斜和旋转的意思，英文里一般用tilt和torsion表示，倾斜对应我们飞行器也就是机体轴系Z轴方向的变化，旋转对应我们飞行器是指绕机体轴系Z轴的旋转运动，倾转分离就是将倾斜和旋转运动从总的姿态期望中剥离出来。

为什么要进行倾转分离？有以下三个原因：

首先，在飞行器实际飞行中，电机的能力是有限的，除了用来平衡飞行器本身的重力之外，留给姿态机动的空间就不是特别充足。而倾斜和旋转两种运动的原理是不同的，倾斜运动是由不同电机差速运动产生的升力差形成的力矩驱使的，而旋转运动是由不同电机差速运动产生的反扭矩驱使的，前者在数值上往往要比后者大很多。

其次，由于绕Z轴的转动惯量也比绕X、Y轴的转动惯量要大得多，所以要改变旋转运动比改变倾斜运动的难度要大很多，所需要占用电机的控制也要大得多。在这种情况下，我们往往需要对旋转运动进行一定的限制，避免在机动时出现电机饱和的情况。

最后，多旋翼飞行器一般是沿X轴和Y轴对称，所以航向朝向哪边真没那么重要，所以在姿态控制中优先保证倾斜也是合情合理的。为此PX4中还特别设定了在航线飞行时保持航向不变的飞行模式。

姿态控制器的代码在文件Firmware\src\modules\mc_att_control\AttitudeControl\AttitudeControl.cpp的update函数中

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
{
  // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN
  q.normalize();
  qd.normalize();

  // calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
  const Vector3f e_z = q.dcm_z();
  const Vector3f e_z_d = qd.dcm_z();
  Quatf qd_red(e_z, e_z_d);

  if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
    // In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
    // full attitude control anyways generates no yaw input and directly takes the combination of
    // roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
    qd_red = qd;

  } else {
    // transform rotation from current to desired thrust vector into a world frame reduced desired attitude
    qd_red *= q;
  }

  // mix full and reduced desired attitude
  Quatf q_mix = qd_red.inversed() * qd;
  q_mix *= math::signNoZero(q_mix(0));
  // catch numerical problems with the domain of acosf and asinf
  q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
  q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
  qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

  // quaternion attitude control law, qe is rotation from q to qd
  const Quatf qe = q.inversed() * qd;

  // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
  // also taking care of the antipodal unit quaternion ambiguity
  const Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();

  // calculate angular rates setpoint
  matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

  // Feed forward the yaw setpoint rate.
  // yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,
  // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
  // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
  // and multiply it by the yaw setpoint rate (yaw_sp_move_rate).
  // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
  // such that it can be added to the rates setpoint.
  rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;

  // limit rates
  for (int i = 0; i < 3; i++) {
    rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
  }

  return rate_setpoint;
}

注释翻译成中文

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward)
{
  // 确保输入四元数严格归一化，因为acosf(1.00001)会得到NaN
  q.normalize();
  qd.normalize();

  // 计算忽略飞行器偏航的简化期望姿态，优先控制横滚和俯仰
  const Vector3f e_z = q.dcm_z();
  const Vector3f e_z_d = qd.dcm_z();
  Quatf qd_red(e_z, e_z_d);

  if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
    // 在极端情况下（飞行器与推力方向完全相反时），完整姿态控制仍不会生成偏航输入
    // 直接采用横滚和俯仰组合仍能得到正确期望偏航。即使忽略此情况也完全安全稳定
    qd_red = qd;

  } else {
    // 将当前到期望推力矢量的旋转转换为世界坐标系下的简化期望姿态
    qd_red *= q;
  }

  // 混合完整和简化的期望姿态
  Quatf q_mix = qd_red.inversed() * qd;
  q_mix *= math::signNoZero(q_mix(0));
  // 处理acosf和asinf定义域可能出现的数值问题
  q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
  q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
  qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

  // 四元数姿态控制律，qe表示从q到qd的旋转
  const Quatf qe = q.inversed() * qd;

  // 使用sin(alpha/2)缩放的旋转轴作为姿态误差（参见轴角定义的四元数）
  // 同时处理对映单位四元数歧义问题
  const Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();

  // 计算角速率设定值
  matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

  // 前馈偏航设定速率
  // yaw_sp_move_rate是世界坐标系z轴的前馈旋转命令
  // 但需要在机体坐标系应用（因为_rates_sp在机体坐标系表达）
  // 通过取R转置的最后一行（即q.inversed）推断世界z轴（机体坐标系表达）
  // 并与偏航设定速率相乘，得到机体坐标系中表示的世界z轴旋转命令矢量
  rate_setpoint += q.inversed().dcm_z() * yawspeed_feedforward;

  // 速率限幅
  for (int i = 0; i < 3; i++) {
    rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
  }

  return rate_setpoint;
}

最新的飞控代码目前是这样

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
	Quatf qd = _attitude_setpoint_q;

	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
	const Vector3f e_z = q.dcm_z();
	const Vector3f e_z_d = qd.dcm_z();
	Quatf qd_red(e_z, e_z_d);

	if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
		// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
		// full attitude control anyways generates no yaw input and directly takes the combination of
		// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
		qd_red = qd;

	} else {
		// Transform rotation from current to desired thrust vector into a world frame reduced desired attitude.
		// This is a right multiplication as the tilt error quaternion is obtained from two Z vectors expressed in the world frame.
		qd_red *= q;
	}

	// With a full desired attitude given by: qd = qd_red * qd_dyaw, extract the delta yaw component.
	// By definition, the delta yaw quaternion has the form (cos(angle/2), 0, 0, sin(angle/2))
	Quatf qd_dyaw = qd_red.inversed() * qd;
	qd_dyaw.canonicalize();
	// catch numerical problems with the domain of acosf and asinf
	qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f);
	qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f);

	// scale the delta yaw angle and re-combine the desired attitude
	qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3))));

	// quaternion attitude control law, qe is rotation from q to qd
	const Quatf qe = q.inversed() * qd;

	// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
	// also taking care of the antipodal unit quaternion ambiguity
	const Vector3f eq = 2.f * qe.canonical().imag();

	// calculate angular rates setpoint
	Vector3f rate_setpoint = eq.emult(_proportional_gain);

	// Feed forward the yaw setpoint rate.
	// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
	// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
	// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
	// such that it can be added to the rates setpoint.
	if (std::isfinite(_yawspeed_setpoint)) {
		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
	}

	// limit rates
	for (int i = 0; i < 3; i++) {
		rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
	}

	return rate_setpoint;
}

注释翻译成中文

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
    Quatf qd = _attitude_setpoint_q;

    // 计算忽略飞行器偏航的简化期望姿态，优先控制横滚和俯仰
    const Vector3f e_z = q.dcm_z();
    const Vector3f e_z_d = qd.dcm_z();
    Quatf qd_red(e_z, e_z_d);

    if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
        // 在极端情况下（飞行器与推力方向完全相反时），完整姿态控制仍不会生成偏航输入
        // 直接采用横滚和俯仰组合仍能得到正确期望偏航。即使忽略此情况也完全安全稳定
        qd_red = qd;

    } else {
        // 将当前到期望推力矢量的旋转转换为世界坐标系下的简化期望姿态
        // 使用右乘方式，因为倾斜误差四元数由世界坐标系下的两个Z向量计算得到
        qd_red *= q;
    }

    // 完整期望姿态定义为 qd = qd_red * qd_dyaw，提取其中的偏航增量分量
    // 根据定义，偏航增量四元数形式为 (cos(angle/2), 0, 0, sin(angle/2))
    Quatf qd_dyaw = qd_red.inversed() * qd;
    qd_dyaw.canonicalize();
    // 处理acosf和asinf定义域可能出现的数值问题
    qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f);
    qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f);

    // 缩放偏航增量角度并重新组合期望姿态
    qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3))));

    // 四元数姿态控制律，qe表示从q到qd的旋转
    const Quatf qe = q.inversed() * qd;

    // 使用sin(alpha/2)缩放的旋转轴作为姿态误差（参见轴角定义的四元数）
    // 同时处理对映单位四元数歧义问题
    //maxi add, eq应该就是error四元数的意思,代表姿态误差  
    const Vector3f eq = 2.f * qe.canonical().imag();

    // 计算角速率设定值
    Vector3f rate_setpoint = eq.emult(_proportional_gain);

    // 前馈偏航设定速率
    // yawspeed_setpoint是世界坐标系z轴的前馈旋转命令
    // 但需要在机体坐标系应用（因为_rates_sp在机体坐标系表达）
    // 通过取R转置的最后一行（即q.inversed）推断世界z轴（机体坐标系表达）
    // 并与偏航设定速率相乘，得到机体坐标系中表示的世界z轴旋转命令矢量
    if (std::isfinite(_yawspeed_setpoint)) {
        rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
    }

    // 速率限幅
    for (int i = 0; i < 3; i++) {
        rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
    }

    return rate_setpoint;
}

官方注释这里明确说了期望的角速度是在body系下的

在代码中，qd_red *= q; 的作用是将从当前姿态到期望姿态的简化旋转（仅考虑俯仰和滚转）转换为世界坐标系下的姿态描述。以下是详细解析：

1. 四元数旋转的组合规则
   四元数的乘法遵循 右乘优先 的规则。例如：
   若四元数 q1 表示从坐标系 A 到 B 的旋转，q2 表示从 B 到 C 的旋转，则组合后的旋转 q2 * q1 表示从 A 到 C 的旋转（先应用 q1，再应用 q2）。
2. qd_red 的初始含义
   qd_red 是通过 Quatf(e_z, e_z_d) 构造的，其中：
   e_z 是当前姿态的 z 轴方向（体坐标系 B → 世界坐标系 W）。
   e_z_d 是期望姿态的 z 轴方向（世界坐标系 W）。
   这表示 qd_red 是 体坐标系下的旋转，仅保留俯仰和滚转（忽略偏航），将当前姿态的 z 轴旋转到期望姿态的 z 轴。
3. qd_red *= q 的作用
   当前姿态四元数 q：表示从世界坐标系（W）到体坐标系（B）的旋转，即 q = W→B。
   qd_red 的初始旋转：表示从体坐标系（B）到简化期望姿态（D）的旋转，即 qd_red = B→D。
   组合后的旋转：qd_red *= q 等价于 qd_red = qd_red * q，即：
   先应用 q 的旋转（从 W 到 B）。
   再应用 qd_red 的旋转（从 B 到 D）。
   结果：组合后的旋转 W→B→D，即 W→D，表示 世界坐标系下的简化姿态。
4. 为何需要转换为世界坐标系？
   混合姿态的需求：后续代码需要将全姿态（包含偏航）与简化姿态（仅俯仰/滚转）在世界坐标系下进行混合（通过权重 _yaw_w）。
   统一坐标系：若 qd_red 保持在体坐标系下，混合时会引入偏航角的耦合问题；转换为世界坐标系后，可直接与全姿态 qd 对比，动态调整偏航权重。

在上述代码中，最终输出的期望角速度是机体坐标系下的。以下是具体分析：

1. 角速度设定值的坐标系
   输出结果：rate_setpoint 是机体坐标系下的角速度设定值。
   关键依据：
   姿态误差计算：eq 是通过四元数误差 qe 的虚部（旋转轴）和符号处理得到的，其物理意义是 机体坐标系下的角速度误差。
   前馈转换：q.inversed().dcm_z() 将世界坐标系的 Z 轴投影到机体坐标系，再与偏航速率前馈值相乘，确保前馈项也在机体坐标系下。
2. 转换到机体坐标系的过程
   (1) 姿态误差转换
   四元数误差 qe：qe = q.inversed() * qd 表示从当前姿态 q 到期望姿态 qd 的旋转。
   误差向量 eq：eq = 2 * qe.imag() 将四元数误差转换为轴角形式（旋转轴 × sin(θ/2)），并乘以 2 近似为小角度角速度（θ ≈ 2 sin(θ/2)）。
   坐标系：由于 q 是当前姿态（世界→机体坐标系），qe 的误差自然表示为 机体坐标系下的角速度。
   (2) 偏航前馈转换
   世界 Z 轴到机体坐标系：q.inversed().dcm_z() 获取当前姿态下世界 Z 轴在机体坐标系中的方向。
   前馈叠加：yawspeed Feedforward 是世界坐标系下的偏航速率，通过与 q.inversed().dcm_z() 相乘，转换为 机体坐标系下的角速度分量。

这个AttitudeControl::update函数在void MulticopterAttitudeControl::Run()里被调用
 

传入的参数q就是无人机当前姿态四元数

    Quatf qd_red(e_z, e_z_d);  

这句对应的构造函数在这里
PX4-Autopilot-main\PX4-Autopilot-main\src\lib\matrix\matrix\Quaternion.hpp

deepseek对这段缩放偏航再重新组合成期望旋转姿态的操作的解释

    // 完整期望姿态定义为 qd = qd_red * qd_dyaw，提取其中的偏航增量分量
    // 根据定义，偏航增量四元数形式为 (cos(angle/2), 0, 0, sin(angle/2))
    Quatf qd_dyaw = qd_red.inversed() * qd;
    qd_dyaw.canonicalize();
    // 处理acosf和asinf定义域可能出现的数值问题
    qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f);
    qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f);

    // 缩放偏航增量角度并重新组合期望姿态
    qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3))));

在PX4的姿态控制代码中，缩放偏航角度的核心目的是 解耦姿态控制自由度，优先保证横滚/俯仰的动态响应，同时避免偏航控制对系统稳定性的负面影响。以下是具体原因和设计逻辑：

1. 物理背景：多旋翼的动力学特性

多旋翼飞行器的姿态控制中：

• 横滚（Roll）和俯仰（Pitch）：直接控制推力矢量方向，影响飞行稳定性，需要快速响应。
• 偏航（Yaw）：仅控制绕垂直轴的旋转，对飞行稳定性影响较小，但容易与其他自由度耦合（尤其在高速旋转时）。

如果偏航控制带宽与横滚/俯仰相同，可能导致：

• 偏航的快速调整干扰横滚/俯仰控制（耦合效应）。
• 执行器饱和（如电机转速剧烈变化）。
• 系统振荡（高频偏航响应引发共振）。

2. 数学操作解析

代码中关键步骤如下：

// 从分解出的纯偏航四元数 qd_dyaw 提取角度
float angle_half = acosf(qd_dyaw(0));  // 四元数实部对应 cos(θ/2)
float scaled_angle_half = _yaw_w * angle_half; 

// 重新构造缩放后的偏航四元数
Quatf scaled_qd_dyaw(
    cosf(scaled_angle_half), 
    0.f, 
    0.f, 
    sinf(scaled_angle_half)  // 虚部对应旋转轴（此处为Z轴）
);

作用解释：

• _yaw_w 参数（通常 0 ≤ _yaw_w ≤ 1）：
  • _yaw_w = 1：偏航角度完全跟踪期望值（无缩放）。
  • _yaw_w < 1：缩小偏航角度误差，降低偏航控制的响应速度。
  • 本质是缩小偏航控制的带宽，使其滞后于横滚/俯仰。

3. 设计目标

a. 优先级控制

• 横滚/俯仰优先：通过缩放偏航，确保横滚/俯仰的误差被优先消除。
• 避免耦合振荡：偏航的慢速响应减少对其他自由度的干扰。

b. 抗饱和与平滑性

• 大角度偏航指令可能导致电机差速剧烈变化（如航拍中的快速偏航）。
• 缩放后，偏航指令的幅值被限制，避免执行器饱和。

c. 参数化调节

• 通过 _yaw_w 参数，开发者可以：
  • 平衡 偏航跟踪精度 与 系统稳定性。
  • 适配不同机型（如大型无人机 vs 小型穿越机）。

4. 实例分析

假设期望偏航角误差为 30°：

• 未缩放（_yaw_w = 1）：控制器尝试完全消除30°误差。
• 缩放（_yaw_w = 0.5）：控制器仅响应 15° 的等效误差。

效果：

• 横滚/俯仰快速收敛 → 飞行器迅速稳定姿态。
• 偏航缓慢跟踪 → 避免因快速偏航调整导致姿态震荡。

5. 与代码其他部分的关联

• 前馈补偿（_yawspeed_setpoint）：

  • 若需要快速偏航响应（如手动操控），可通过前馈直接注入偏航速率指令。
  • 缩放仅作用于反馈环节，不影响前馈路径。

• 四元数分解逻辑：

  • 通过分离横滚/俯仰（qd_red）和偏航（qd_dyaw），实现物理意义的解耦。
  • 缩放操作仅在偏航分量上进行，保持横滚/俯仰的完整性。

6. 调参建议

• 默认值：通常 _yaw_w = 0.5（保守平衡）。
• 增大 _yaw_w：需要更精准的偏航跟踪（如航拍构图）。
• 减小 _yaw_w：优先稳定性（如载重运输、强风环境）。

总结

缩放偏航角度是为了让偏航控制的响应速度低于横滚/俯仰，从而优先保证飞行稳定性，同时避免多自由度耦合带来的振荡风险。这一设计体现了多旋翼控制中“分层优先级”和“带宽管理”的核心思想。

为什么输出的期望角速度是机体系下的，这里做了说明。
【飞控简析-从入门到跑路 第一章PX4的姿态控制(2) - CSDN App】https://blog.csdn.net/weixin_38693938/article/details/83794666?sharetype=blog&shareId=83794666&sharerefer=APP&sharesource=sinat_16643223&sharefrom=link

具体代码对应的这句,qe实际就是机体系下的期望角度了。

    // 四元数姿态控制律，qe表示从q到qd的旋转
    const Quatf qe = q.inversed() * qd;

如果偏航不对，那么这句里面的当前姿态q不对，期望姿态qd？没有问题，进而会导致qe不对，得到的期望角速度也就不对，造成定点定不住。

注意前面这里虽然用到了q，但是因为计算的是z轴方向，所以即使偏航不对，e_z 也不受影响，所以偏航不对，q真正产生影响的还是 const Quatf qe = q.inversed() * qd; 这句。

  // 计算忽略飞行器偏航的简化期望姿态，优先控制横滚和俯仰
  const Vector3f e_z = q.dcm_z();
  const Vector3f e_z_d = qd.dcm_z();
  Quatf qd_red(e_z, e_z_d);

2 当前姿态的解算

根据陀螺仪的角度数据高频特性好，而加速度计和地磁计得到的角度数据低频特性好，从而进行互补，得到最优角度。无论什么算法，本质都是陀螺仪积分得到角度，然后根据加速度计和地磁计修正积分的漂移误差。 ( https://mp.weixin.qq.com/s/IrmJMW1fzD4uyTRtdehe7g )

• 陀螺仪用于短时姿态预测。
  • 加速度计校正重力方向（Roll/Pitch）。
  • 磁力计校正地磁方向（Yaw）。

通过vehicle_attitude主题发布NED姿态，供控制器使用。

1. 传感器数据预处理

传感器数据（IMU、磁力计, 视觉位姿）通过uORB主题订阅进入EKF2模块，视觉位姿是订阅的vehicle_visual_odometry这个uorb消息：

// src/modules/ekf2/ekf2_main.cpp
void Ekf2::Run()
{
    // 订阅IMU数据（机体坐标系，通常为FRD）
    _imu_sub = orb_subscribe(ORB_ID(vehicle_imu));
    // 订阅磁力计数据
    _magnetometer_sub = orb_subscribe(ORB_ID(vehicle_magnetometer));
    // ...其他传感器订阅
}

2. EKF2初始化与状态变量

EKF2维护状态变量，包含四元数（q）、速度、位置等：

// src/lib/ecl/EKF/ekf.h
class Ekf {
protected:
    Quatf _state.quat_nominal; // 机体到NED坐标系的旋转四元数
    Vector3f _state.vel;       // NED速度
    Vector3f _state.pos;       // NED位置
    // ...
};

3. 预测步骤（陀螺仪积分）

使用陀螺仪数据预测姿态：

// src/lib/ecl/EKF/ekf.cpp
void Ekf::predictState()
{
    // 陀螺仪测量值（机体坐标系）
    Vector3f ang_vel = _imu_sample_delayed.delta_ang / _imu_sample_delayed.delta_ang_dt;
    
    // 四元数积分预测姿态
    _state.quat_nominal = quatPredict(_state.quat_nominal, ang_vel, dt);
    // ...协方差预测
}

4. 更新步骤（传感器融合）

加速度计更新（校正Roll/Pitch）

void Ekf::controlFusionModes()
{
    // 重力向量在NED坐标系为[0, 0, 9.81]
    Vector3f gravity_ned(0.f, 0.f, 9.81f);
    
    // 将预测的重力转换到机体坐标系
    Vector3f predicted_gravity = _state.quat_nominal.conjugate(gravity_ned);
    
    // 加速度计测量值与预测值的残差
    Vector3f accel_error = _accel_sample_delayed.data - predicted_gravity;
    
    // 使用EKF更新状态
    updateAccelerometer(accel_error);
}

磁力计更新（校正Yaw）

void Ekf::controlMagFusion()
{
    // 地磁参考向量（NED坐标系）
    Vector3f mag_field_ned = _mag_earth_predicted;
    
    // 转换预测的磁场到机体坐标系
    Vector3f predicted_mag = _state.quat_nominal.conjugate(mag_field_ned);
    
    // 磁力计测量值与预测值的残差
    Vector3f mag_error = _mag_sample_delayed.data - predicted_mag;
    
    // 使用EKF更新状态
    updateMag(mag_error);
}

. EKF2视觉偏航融合

在EKF的更新阶段，优先使用视觉提供的偏航角：

// src/lib/ecl/EKF/ekf.cpp
void Ekf::controlExternalVisionFusion()
{
    // 检查视觉数据有效性
    if (_ev_sample_delayed.time_us != 0 && isNewestSampleRecent(_time_last_ext_vision_buffer_push)) {

        // 提取视觉四元数（NED坐标系）
        Quatf q_ev(_ev_sample_delayed.quat);

        // 计算视觉偏航角
        const Eulerf euler_ev(q_ev);
        const float yaw_ev = euler_ev(2); // 视觉提供的偏航角（弧度）

        // 视觉偏航融合
        if (_control_status.flags.ev_yaw) {
            // 计算视觉偏航与当前估计的残差
            float yaw_innov = yaw_ev - getEulerYaw(_state.quat_nominal);
            
            // 使用EKF更新偏航状态
            fuseYaw(yaw_innov, _ev_sample_delayed.angVar); // 残差和方差
        }
    }
}

. 禁用磁力计偏航更新

在磁力计更新逻辑中跳过偏航校正：

// src/lib/ecl/EKF/ekf.cpp
void Ekf::controlMagFusion()
{
    // 如果启用了视觉偏航融合，则跳过磁力计偏航更新
    if (_control_status.flags.ev_yaw) {
        return; 
    }
    
    // 否则继续使用磁力计校正偏航
    // ...原有磁力计更新代码
}

5. 发布NED姿态

EKF2将最终姿态发布到vehicle_attitude主题：

// src/modules/ekf2/ekf2_main.cpp
void Ekf2::publishAttitude()
{
    vehicle_attitude_s att;
    
    // 四元数（机体到NED）
    att.q[0] = _ekf.getQuaternion()(0);
    att.q[1] = _ekf.getQuaternion()(1);
    att.q[2] = _ekf.getQuaternion()(2);
    att.q[3] = _ekf.getQuaternion()(3);
    
    // 欧拉角（Roll, Pitch, Yaw）
    att.roll = math::degrees(_ekf.getEulerAngles()(0));
    att.pitch = math::degrees(_ekf.getEulerAngles()(1));
    att.yaw = math::degrees(_ekf.getEulerAngles()(2));
    
    // 发布到uORB
    _att_pub.publish(att);
}

实际我搜到的函数是这个

bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime &now)
{
	if (_ekf.attitude_valid()) {
		// generate vehicle attitude quaternion data
		vehicle_attitude_s att;
		att.timestamp = now;

		const Quatf q{_ekf.calculate_quaternion()};
		q.copyTo(att.q);

		_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);

		_att_pub.publish(att);

		return true;

	}  else if (_replay_mode) {
		// in replay mode we have to tell the replay module not to wait for an update
		// we do this by publishing an attitude with zero timestamp
		vehicle_attitude_s att{};
		_att_pub.publish(att);
	}

	return false;
}

视觉位姿进入飞控的过程

假设从机载端电脑或者其他信息可以得到视觉SLAM输出的位姿，可以通过mavlink 协议的VISION_POSITION_ESTIMATE或者ODOMETRY消息输入给飞控，区别在于VISION_POSITION_ESTIMATE只包含位置信息，而ODOMETRY还可以得到速度、姿态和姿态角速度等。 飞控中对应的代码处理在:https://github.com/cloudkernel-tech/Firmware/blob/master_kerloud/src/modules/mavlink/mavlink_receiver.cpp对应的函数为:handle_message_vision_position_estimate()和handle_message_odometry()。得到的SLAM信息会通过uorbtopic vehicle_visual_odometry发布出来。 EKF2模块(src/modules/ekf2/ekf2_main.cpp)在line 1127开始会接受vehicle_visual_odometrytopic的值，给对应位置、速度或者姿态赋值。 External visioninnovation 计算：水平方向计算在:src/lib/ecl/EKF/control.cpp的line 307竖直方向计算在:vel_pos_fusion.cpp的line 150Innovationvariance计算在vel_pos_fusion.cpp的line 167，对应K值修正计算在line 234行，最后的状态修正在line 315行。

https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/yaw_fusion.cpp

还找到一个这个
https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/yaw_fusion.cpp

#if defined(CONFIG_EKF2_EXTERNAL_VISION)

	// update EV attitude error filter
	if (_ev_q_error_initialized) {
		const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
		_ev_q_error_filt.reset(ev_q_error_updated);
	}

#endif // CONFIG_EKF2_EXTERNAL_VISION

yaw_fusion.cpp是PX4的EKF2（扩展卡尔曼滤波器）模块的核心文件之一，主要负责处理偏航角（Yaw）的多传感器融合逻辑。其核心目标是将来自不同传感器（如磁力计、GPS、外部视觉系统等）的偏航数据与EKF内部状态估计相结合，以提高无人机姿态估计的准确性和鲁棒性。

又发现一个这个
 

里面所调用的fuseYaw函数正是之前找到的yaw_fusion.cpp里面定义的。
 

下面是让deepseek对fuseYaw函数做的分析
Ekf::fuseYaw 函数是PX4 EKF中用于融合偏航角（Yaw）测量的核心函数。以下是对该函数的逐步解析：

1. 函数参数与作用

• 输入参数：
  • aid_src_status：包含辅助源的状态信息（如新息、方差、拒绝标志等）。
  • H_YAW：偏航角的观测矩阵（雅可比矩阵），表示状态对测量的敏感度。
• 功能：将偏航角测量值通过卡尔曼滤波更新到状态估计中，处理数值稳定性问题，并根据条件决定是否融合。

2. 协方差矩阵健康检查

if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
    _fault_status.flags.bad_hdg = false;
} else {
    _fault_status.flags.bad_hdg = true;
    initialiseCovariance();  // 重置协方差矩阵
    return false;            // 终止融合
}

• 目的：确保新息方差（来自状态协方差）不小于观测方差。
• 逻辑：
  • 如果新息方差小于观测方差，说明协方差矩阵病态（可能负定），需重置协方差矩阵并报错。
  • 设置故障标志 bad_hdg，防止后续依赖此状态的融合。

3. 卡尔曼增益计算

VectorState Kfusion;
const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance;

for (uint8_t row = 0; row < State::size; row++) {
    for (uint8_t col = 0; col <= 3; col++) {
        Kfusion(row) += P(row, col) * H_YAW(col);
    }
    Kfusion(row) *= heading_innov_var_inv;
}

• 目的：计算卡尔曼增益 Kfusion。
• 细节：
  • 仅遍历状态的前4列（col <= 3），可能因 H_YAW 仅在姿态四元数或陀螺偏置相关状态处非零。
  • P 是状态协方差矩阵，H_YAW 提取对应状态的灵敏度。
  • 最终增益为 K = P * H^T / innovation_variance。

4. 新息拒绝处理

if (aid_src_status.innovation_rejected) {
    _innov_check_fail_status.flags.reject_yaw = true;

    if (!_control_status.flags.in_air 
        && isTimedOut(_time_last_in_air, 5e6) 
        && isTimedOut(aid_src_status.time_last_fuse, 1e6)) 
    {
        // 地面状态：约束新息并重置陀螺Z轴协方差
        const float gate_limit = sqrtf(sq(gate_sigma) * aid_src_status.innovation_variance);
        aid_src_status.innovation = constrain(..., -gate_limit, gate_limit);
        resetGyroBiasZCov();
    } else {
        return false; // 空中状态拒绝融合
    }
} else {
    _innov_check_fail_status.flags.reject_yaw = false;
}

• 目的：处理新息超出置信区间的情况（如卡方检验失败）。
• 逻辑：
  • 空中状态：直接拒绝融合，避免错误测量影响估计。
  • 地面状态：
    • 约束新息在门限内（gate_sigma 来自参数，至少1σ）。
    • 重置陀螺偏置Z轴的协方差，加速陀螺偏置收敛（假设地面时偏航角较稳定）。

5. 测量更新与状态记录

measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation);
_time_last_heading_fuse = _time_delayed_us;
aid_src_status.time_last_fuse = _time_delayed_us;
aid_src_status.fused = true;
_fault_status.flags.bad_hdg = false;
return true;

• 目的：执行卡尔曼滤波的测量更新，记录时间戳和融合状态。
• 关键函数：
  • measurementUpdate()：更新状态向量和协方差矩阵。
  • 重置 bad_hdg 标志，标记融合成功。

6. 设计要点总结

• 数值稳定性：通过协方差重置避免病态矩阵问题。
• 地面/空中逻辑分离：地面允许强制融合以应对初始偏差或干扰。
• 新息门限约束：防止异常测量破坏估计，同时通过协方差重置加速收敛。
• 状态针对性更新：仅更新与偏航相关的状态（通过 H_YAW 稀疏性优化计算）。

7. 关键参数与函数

• 参数：
  • heading_innov_gate：新息门限（如5σ）。
  • observation_variance：偏航测量的传感器噪声方差。
• 辅助函数：
  • initialiseCovariance()：重置协方差矩阵到初始值。
  • resetGyroBiasZCov()：重置陀螺Z轴偏置的协方差。

此函数体现了EKF在实际系统中的工程考量，如数值鲁棒性、状态机切换（地面/空中）、传感器异常处理等。

在EKF融合前，有先根据偏航做变换。这是合理的，也应该是这样，不可能直接去和之前的姿态融合啊。
 

deepseek对Ekf::resetQuatStateYaw函数中的一段基于新给的偏航做旋转的代码解析。
更新旋转矩阵_R_to_earth**

_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));

• updateYawInRotMat 函数：
  • 输入新的偏航角yaw和当前四元数转换的方向余弦矩阵（DCM）。
  • 保持横滚（Roll）和俯仰（Pitch）不变，仅替换偏航角，生成新的DCM。
• 结果_R_to_earth是新旋转矩阵。

这段代码的作用是更新旋转矩阵（DCM）中的偏航角（Yaw），同时保持横滚角（Roll）和俯仰角（Pitch）不变。具体解析如下：

函数功能

• 输入：原始旋转矩阵 rot_in 和新的偏航角 yaw。
• 输出：更新后的旋转矩阵，仅修改偏航角，保持横滚和俯仰角不变。
• 核心操作：通过欧拉角（321顺序）分解旋转矩阵，替换偏航角后重新构造旋转矩阵。

代码逐行解析

1. 分解原始旋转矩阵为欧拉角（321顺序）

matrix::Euler<T> euler321(rot_in);

• matrix::Euler<T> 是欧拉角类，此处使用 321顺序（即 Z-Y-X 顺序）：
  • 先绕 Z轴 旋转（偏航角 Yaw），
  • 再绕 Y轴 旋转（俯仰角 Pitch），
  • 最后绕 X轴 旋转（横滚角 Roll）。
• 通过构造函数 euler321(rot_in)，将旋转矩阵 rot_in 分解为 321 顺序的欧拉角。

2. 替换偏航角

euler321(2) = yaw;

• euler321 的索引含义：
  • 索引 0: 横滚角（Roll，绕 X 轴）
  • 索引 1: 俯仰角（Pitch，绕 Y 轴）
  • 索引 2: 偏航角（Yaw，绕 Z 轴）
• 此行代码将欧拉角的偏航角（索引 2）替换为新的 yaw 值，同时保留横滚和俯仰角。

3. 重新构造旋转矩阵

return matrix::Dcm<T>(euler321);

• 根据更新后的欧拉角（新的偏航角 + 原始的横滚和俯仰角），重新生成旋转矩阵。
• 关键特性：新矩阵与原矩阵仅在绕 Z 轴的旋转（偏航角）上不同，其他轴旋转保持不变。

数学原理

1. 欧拉角分解
   通过 321 顺序的欧拉角分解，将旋转矩阵拆解为三个独立的角度：Yaw → Pitch → Roll。
2. 替换偏航角
   仅修改绕 Z 轴的旋转角度，保持 X 和 Y 轴的旋转不变。
3. 重建旋转矩阵
   使用新的欧拉角重新构造旋转矩阵，确保姿态更新仅影响偏航方向。

应用场景

• 偏航角重置：当需要强制对齐估计的偏航角（如磁力计校正、视觉定位）时，调用此函数。
• 传感器融合：在 EKF 中，若检测到偏航角需要修正（例如 GPS 或外部视觉提供更可靠的偏航值），通过此函数更新旋转矩阵。

示例

假设原始旋转矩阵 rot_in 对应的欧拉角为 [roll=0.1, pitch=0.2, yaw=0.3]，调用 updateEuler321YawInRotMat(0.5, rot_in) 后：

1. 分解出欧拉角：roll=0.1, pitch=0.2, yaw=0.3。
2. 替换偏航角：yaw=0.5。
3. 新欧拉角：[roll=0.1, pitch=0.2, yaw=0.5]。
4. 生成的新旋转矩阵仅绕 Z 轴旋转 0.5 弧度，X 和 Y 轴旋转不变。

注意事项

• 顺序一致性：321 顺序严格遵循 Z-Y-X 旋转，确保分解和重建的逻辑正确。
• 万向节锁：在俯仰角为 ±90° 时，欧拉角分解会出现奇点，需避免在此情况下使用。

matrix::Dcm(euler321)对应的把欧拉角转为旋转矩阵的真正实现代码在这，似乎通过构造函数，模板等什么实现的。
注释部分说明了是内旋的方式转的，内旋的话只改了偏航角，横滚角和俯仰角没变是合理的。
PX4-Autopilot-main\PX4-Autopilot-main\src\lib\matrix\matrix\Dcm.hpp
 

这段话描述的是如何构建从坐标系2到坐标系1的变换矩阵，其中坐标系1到坐标系2的旋转由3-2-1内在Tait-Bryan旋转序列定义。以下是关键点解析：
deepseek对英文注释的分析

1. 3-2-1内在旋转序列

• 轴顺序：3-2-1对应Z-Y-X轴的旋转顺序（第三轴→第二轴→第一轴）。
• 内在旋转：每次旋转均绕当前坐标系的轴进行，而非固定参考系。例如：
  1. 绕初始坐标系（框架1）的Z轴旋转（偏航角，Yaw）。
  2. 绕新坐标系（第一次旋转后的）的Y轴旋转（俯仰角，Pitch）。
  3. 绕最新坐标系（第二次旋转后的）的X轴旋转（滚转角，Roll）。

2. 旋转矩阵的构造

• 从框架1到框架2的旋转矩阵 R₁→₂ 由三个基本旋转矩阵按顺序右乘构成：
  [
  R_{1→2} = R_Z(ψ) \cdot R_Y(θ) \cdot R_X(φ)
  ]
  其中，(ψ, θ, φ) 分别为绕Z、Y、X轴的旋转角度。

3. 变换矩阵从框架2到框架1

• 旋转部分的逆：
  从框架2到框架1的旋转矩阵是 R₁→₂ 的逆矩阵（即转置，因为旋转矩阵是正交矩阵）：
  [
  R_{2→1} = R_{1→2}^T = R_X(-φ) \cdot R_Y(-θ) \cdot R_Z(-ψ)
  ]
  这等价于按相反顺序（X-Y-Z）并以负角度进行旋转。

4. 物理意义

• 框架1→2的旋转：通过Z-Y-X顺序依次旋转，将点从框架1转换到框架2。
• 框架2→1的变换：通过逆过程（X-Y-Z顺序，反向旋转）将点从框架2还原到框架1。

示例

若框架1到框架2的旋转角度为 (ψ=30°, θ=45°, φ=60°)，则：

• 框架1→2的旋转矩阵：
  [
  R_{1→2} = R_Z(30°) \cdot R_Y(45°) \cdot R_X(60°)
  ]
• 框架2→1的旋转矩阵：
  [
  R_{2→1} = R_X(-60°) \cdot R_Y(-45°) \cdot R_Z(-30°)
  ]

总结

这段话的核心是：3-2-1内在旋转序列（Z-Y-X）定义了框架1到框架2的旋转，而从框架2到框架1的变换矩阵需通过逆序（X-Y-Z）且角度取反的旋转矩阵实现。这种变换矩阵广泛应用于机器人学、航空航天等领域，用于坐标系间的姿态转换。

旋转矩阵转欧拉角时，可以看到也是依照内旋的方式转的。 对应matrix::Euler euler321(rot_in);这句代码
PX4-Autopilot-main\PX4-Autopilot-main\src\lib\matrix\matrix\Euler.hpp

所以现在可以确定，得到视觉位姿的偏航之后，整个姿态都做了对应的旋转。

关键是把EKF模块 订阅视觉位姿信息 到发布localposition和位姿四元数消息 这中间流程发生了什么理清楚，经过了哪些cpp哪些函数处理。
控制模块也可以加进来，画出一个代码上的流程框架图。类似于ROS里的节点图。

我们可以一点点顺着理消息流
SLAM信息会通过uorbtopic vehicle_visual_odometry发布出来。 EKF2模块(src/modules/ekf2/ekf2_main.cpp)在line 1127开始会接受vehicle_visual_odometrytopic的值，给对应位置、速度或者姿态赋值。

有个细节说一下，高飞没有把vins位姿发给飞控，那它的偏航怎么来的，我们还给他发期望姿态，我看了当初备份的他们的飞控参数，EKF2_MAG_TYPE的值为6，意思就是偏航
https://docs.px4.io/main/zh/advanced_config/parameter_reference.html
 

3 mavros坐标系，参考之前文章

它这里考虑到了横滚俯仰误差，一般有重力对齐的SLAM位姿应该还好。
You can check these MAVLink messages with the QGroundControl MAVLink Inspector
(opens new window) In order to do this, yaw the vehicle until the quaternion of the ODOMETRY message is very close to a unit quaternion. (w=1, x=y=z=0)

At this point the body frame is aligned with the reference frame of the external pose system. If you do not manage to get a quaternion close to the unit quaternion without rolling or pitching your vehicle, your frame probably still have a pitch or roll offset. Do not proceed if this is the case and check your coordinate frames again.
https://docs.px4.io/v1.14/zh/ros/external_position_estimation.html

参考资料:
PX4开源飞控位置控制原理 https://mp.weixin.qq.com/s/Dpza0NwFP_lpbI-ys9z-qg
基于飞控的姿态估计算法详解 https://mp.weixin.qq.com/s/IrmJMW1fzD4uyTRtdehe7g
Kerloud PX4飞控的EKF2程序导航 https://mp.weixin.qq.com/s/oZUtYYp7013etqdoYD-xRg