关于 n3ctrl 中的代码疑问

[ghvn7777]

首先非常感谢你们的工作，我想把移植到 px4 上，阅读代码一些疑问，想请教一下。

看 n3ctrl 的 readme.md 中对于推力的说明为：

Kp * e_p + Kv * e_v + Kv_i * ∫ e_v + Ka * des_a

而在实际代码中我看 Controller::update() 函数中计算推力有这么几行：

e_p = des.p - odom.p;
Eigen::Vector3d u_p = wRc * Kp * cRw * e_p;

e_v = des.v + u_p - odom.v;
u_v_p = wRc * Kv * cRw * e_v;

首先是不太理解为什么要乘以 yaw 角构成的旋转矩阵，其次不太理解 e_v 为啥和 u_p 有关系，我的理解是这样：

e_p = des.p - odom.p;
e_v = des.v - odom.v;
// 后面有个类似这样的公式 ...
kp * ep + kv * e_v + des.a

期待您的解答～

[bigsuperZZZX]

1. 乘以 yaw 角构成的旋转矩阵是因为Kp和Kv是定义在机体坐标系下的，我们认为飞机在不同方向上（横滚、俯仰）的控制力道是不一样的，从而反馈增益不一样，所以转换到C坐标系（一个只旋转了yaw的坐标系）下做反馈计算。实际使用中乘不乘这个旋转矩阵性能上提升不大，可以不管。

2. u_p放到e_v中是串级PID的思想，这一设计实测可以显著提升控制性能。你的写法也能用，但你试一试就发现效果没有串级PID好了。

[ghvn7777]

好嘞，多谢解答，真是太感谢了～