参考FastDrone-250的px4ctrl模块悬停控制

悬停期望状态：位置+速度全零

// PX4CtrlFSM.cpp:428
Desired_State_t PX4CtrlFSM::get_hover_des() {
des.p = hover_pose.head<3>(); // 目标=捕获点
des.v = Vector3d::Zero(); // 期望速度=0
des.a = Vector3d::Zero(); // 期望加速度=0
des.yaw = hover_pose(3);
des.yaw_rate = 0.0;
}

控制器：PD + 重力前馈

// controller.cpp
des_acc = des.a + Kv.asDiagonal()*(des.v - odom.v)     // D项：速度误差
        + Kp.asDiagonal()*(des.p - odom.p)             // P项：位置误差
        + Vector3d(0, 0, gra);                         // 重力前馈

悬停时 des.a=0, des.v=0, des.p=hover_pose，所以：
des_acc = Kp*(hover_pose - p) + Kv*(0 - v) + (0,0,g)
位置偏差 → P 项拉回
速度偏差 → D 项阻尼（抑制振荡）
重力 → 前馈直接补偿
默认增益 Kp=Kv=1.5（见 yaml），是阻尼比合理的 PD。

推力映射：在线自适应估计

参数thr2acc_是指单位油门比例即100%所能产生的加速度（不含重力）

void 
LinearControl::resetThrustMapping(void)
{
  thr2acc_ = param_.gra / param_.thr_map.hover_percentage;
  P_ = 1e6;
}

由此，throttle_percentage = des_acc(2) / thr2acc_;
thr2acc_ 不是固定值，用递归最小二乘（RLS）在线估计，
输入是35ms～45ms前的油门比例thr与当下的加速度矢量est_a

gamma = 1 / (rho2_ + thr * P_ * thr);
K     = gamma * P_ * thr;
thr2acc_ += K * (est_a(2) - thr * thr2acc_);
P_ = (1 - K*thr) * P_ / rho2_;   // rho2_=0.998 遗忘因子

用 IMU 实测加速度 vs 35~45ms 前发的推力指令（补偿执行延迟）做在线辨识
遗忘因子 0.998 → 缓慢跟踪电池电压下降、推力曲线变化
初始化：thr2acc_ = gra / hover_percentage（hover_percentage 默认 0.10）
效果：悬停时 des_acc(2)≈g，油门自动收敛到 hover_percentage，不管电池电压/桨效怎么变。

姿态解算：微分平坦逆映射

// controller.cpp:39-47
roll = (des_acc(0)*sin(yaw) - des_acc(1)*cos(yaw)) / gra;
pitch = (des_acc(0)*cos(yaw) + des_acc(1)*sin(yaw)) / gra;
q = Rz(des.yaw) * Ry(pitch) * Rx(roll);
u.q = imu.q * odom.q.inverse() * q; // 融合 IMU 与里程计
注意它用 imu.q * odom.q.inverse() * q 做姿态融合，把期望的世界系姿态转成机体系指令。

遥控可微调悬停点（锦上添花）

// PX4CtrlFSM.cpp:502 AUTO_HOVER 状态下每周期执行
void PX4CtrlFSM::set_hov_with_rc() {
hover_pose(0) += rc_data.ch[1] * max_manual_vel * delta_t; // 遥控杆量叠加
hover_pose(1) += rc_data.ch[0] * max_manual_vel * delta_t;
hover_pose(2) += rc_data.ch[2] * max_manual_vel * delta_t;
hover_pose(3) += rc_data.ch[3] * max_manual_vel * delta_t;
}