px4+vins+ego单机鲁棒飞行二（外部位姿估计篇)

参考：px4官方WIKI

一、使用px4的EKF2（扩展卡尔曼）估计

EKF2使用扩展卡尔曼滤波器进行三维的姿态，位置/速度以及风的状态进行估计。

1.1 将QGC中System中的SYS_MC_EST_GROUP设置为1
1.2 启用外部位姿输入：

1. EKF2_AID_MASK=280(vision position fusion、vision yaw fusion、vision velocity fusion)
2. EKF2_HGT_MODE=Vision
   1.3 设置视觉传感器相对于车身的位置
   EKF2_EV_POS_X
   EKF2_EV_POS_Y
   EKF2_EV_POS_Z

测试过程及结果：

1. 刚开始没有使用速度估计，即EKF2_AID_MASK=24
   视觉估计会和px4加速度计估计的差别挺大，导致EKF2融合会出现延迟现象。具体就是飞机急停或者转弯，px4实际融合的位置会延迟或者超调。
2. 将EKF2_AID_MASK改为280，并发送/mavros/vision_speed/speed_vector话题
   没有卵用。。。。。查看rqt_graph，mavros根本就没接收（确定vision_speed_estimate插件在白名单）。官方wiki写的暂时不支持，不知道是不是这个原因。
   
3. 查看官方插件wiki，发现有两个话题
   一个是~vision_speed/speed_vector（就是我上面用的），另一个是vision_speed/speed_twist。发现最后还有个参数设置
   然后查看px4_config.yaml文件中发现原文是这样的：

# vision_speed_estimate
vision_speed:
  listen_twist: true    # enable listen to twist topic, else listen to vec3d topic
  twist_cov: true       # enable listen to twist with covariance topic

发现listen_twist: true是监听第二个话题。那vec3d和twist有什么区别呢？
答：前者只能穿速度，后者可以传linear和angular，即多了一个angular，果断选择前者（因为vins里程计并没有给angular，而且飞控本身的angular其实挺准的）。

4. 更改mavros配置，使用视觉速度估计

4.1 在3中发现了问题之后，更改为：

# vision_speed_estimate
vision_speed:
  listen_twist: false    # enable listen to twist topic, else listen to vec3d topic
  twist_cov: false       # enable listen to twist with covariance topic

4.2 此时再次运行程序，查看rqt_graph，发现话题被成功订阅：
4.3 干，测试好像还是会有那种停下后慢慢融合的样子，速度还是差距好大，不过好像是好了一点丢丢（心里安慰？）。
4.4 监听/mavros/odometry/in，实锤了，linear项全为nan。
4.5 发送/mavros/vision_speed/speed_twist试试？用的摄像头的imu，效果更差。。。。

5. 测试EKF2_EV_POS_X
   将EKF2_EV_POS_X=0.08，因为我的摄像头在飞控前大概8cm的位置，测试成功，mavros返回值初始x为-0.08。

6. 最终解决方案—>发送视觉里程计
   6.1 突然发现可以用发送里程计的方法，即发送/mavros/odometry/out话题的方法（注意frame_id=odom）。然后我就整合上面的发送数据为里程计信息，发送给/mavros/odometry/out，然后监听/mavros/odometry/in，并观察数据，成功！！！
   tf树为：
   
   ps：world和base_link连接是因为我更改了px4_config.yaml文件（但是测试好像这样最开始不太吻合，动几下就好了）。

# local_position
local_position:
  frame_id: "world"
  tf:
    send: true
    frame_id: "world"
    child_frame_id: "base_link"
    send_fcu: true

6.2 rqt_grph为：
6.3 /mavros/odometry/out和/mavros/odometry/in如下：
6.4 发现一个大问题
px4的线速度永远是body系,所以需要使用公式将local系的速度转换为body系，否则yaw轴角度改变后将导致无人机原地画圈。

二、使用px4的LPE估计

LPE (Local Position Estimator) 位置估计使用扩展卡尔曼滤波器对三维位置以及速度进行估计。

2.1 将QGC中System中的SYS_MC_EST_GROUP设置为1（local_positon_estimator）
2.2 重启PX4和QGC，在Parameters中会出现Local Position Estimator配置。
2.3 启用外部位姿输入：

1. LPE_FUSION=4，即只选择fuse vision position，我没有气压计所以不勾选fuse baro。
2. ATT_EXT_HDG_M=Vision，即设置偏航来自视觉。

2.4 优化噪声参数
如果你的视觉或动作捕捉数据非常精确，并且你希望评估者能够紧密地跟踪它，你就应该减少标准偏差参数:LPE_VIS_XY和LPE_VIS_Z(针对VIO)或LPE_VIC_P(针对MoCap)。减少它们将使估计器更信任传入的姿态估计。您可能需要将它们设置为低于允许的最小值并强制保存()。
TIP：如果性能仍然很差，请尝试增加LPE_PN_V参数。这将导致估计器在速度估计过程中更加信任度量值。
TIP：设置LPE_VIS_XY和LPE_VIS_Z时，最小只支持0.01，但是可以输入一个0，点Save，然后下面会出来一个Force save(dangerous)选项，勾选就可以强制更改。

2.5 将视觉估计的结果以/mavros/vision_pose/pose话题发给px4。

测试过程及结果：

1）首先优化噪声参数全部为默认值：

1. 效果还不错，可能会有5厘米以内的误差，然后慢慢同步。
2. /mavros/local_position/odom返回的速度估计会有很大的误差（此时没有发送速度估计）。
3. 偏航角不同（感觉并没有用发送的估计值）
4. 突然发现tf树的world没得了。。。。。啥话题发送都不行
5. 将LPE_VIS_XY和LPE_VIS_Z改到最小之后疯狂跳动。
6. 最终LPE_PN_V=1，LPE_VIS_XY=0.5， LPE_VIS_Z=0.5

三、用EKF2还是LPE？官网这么解释的

在文章中间的External Vision System部分

    与所有估计器一样，大部分表现来自于匹配传感器特性的调优。调优是准确性和鲁棒性之间的折衷，尽管我们试图提供满足大多数用户需求的调优，但仍会有需要进行调优更改的应用程序。（我们其实需要的是准确行，鲁棒性应该在视觉估计中考虑）。
  For this reason, no claims for accuracy (没有精确度的要求???我感觉视觉估计的比你准确) relative to the legacy combination of attitude_estimator_q + local_position_estimator have been made and the best choice of estimator will depend on the application and tuning.

3.1 EKF缺点

1. ecl EKF是一种复杂的算法，需要很好地理解扩展卡尔曼滤波理论及其在导航问题中的应用，才能成功地进行调优。因此，对于没有取得好的结果的用户来说，要知道应该改变什么就更加困难了。
2. ecl EKF使用更多的RAM和闪存空间。
3. ecl EKF使用更多的日志空间。

3.2 优点

1. ecl EKF能够以一种数学上一致的方式融合来自不同时延和数据速率的传感器的数据，一旦时延参数设置正确，就可以提高动态机动时的准确性。
2. 该ecl EKF能够融合大范围的不同类型的传感器。
3. ecl EKF检测并报告传感器数据统计上显著的不一致，协助诊断传感器错误。
4. 对于固定翼操作，ecl EKF在有或没有空速传感器的情况下估计风速，并能够将估计的风速与空速测量和侧滑假设结合使用，以延长GPS在飞行中丢失时可用的方位估算时间。
5. ecl EKF估计了三轴加速度计偏差，以提高尾座和其他在飞行阶段之间经历巨大姿态变化的飞行器的精度。
6. 联合架构(结合姿态和位置/速度估计)意味着姿态估计可以从所有传感器测量中受益。如果调整正确，这将提供改进态度评估的潜力。

3.3总结

这么多优点，好像对我球用没有。

四、更改源码

px4+vins+ego单机鲁棒飞行二-1（更改px4外部视觉估计固件）
结果：px4的位置完全服从外部视觉给位置，唯一缺点就是，相机位置和飞机中心位置在QGC中设置的自动转换没用了，只能自己再写静态tf转换。
这样子不行，这样的位置与px4估计严重不符，px4会切断位置输入。
最终解决方案：自己写位置PID，只用到了飞控的速度控制。还有各种应用层模式也自己写。（效果还不错）
用到的是/mavros/setpoint_raw/local话题，具体的PID就不公布了，大家可以自己写。
使用注意事项

五、2021年12月2日更新

1. 亲测使用速度融合效果更差，EKF2_AID_MASK=280才有效果。
2. 亲测EKF2_AID_MASK=24可以使用，不过默认或有一定的滤波过头。
3. 亲测如果将EKF2_EVV_NOISE设置大一点融合可信度会更高一些，我设置的为EKF2_EVP_NOISE = 0.20m