PX4 代码中 position_estimator_inav(互补滤波)理解

Local_position_estimator 为卡尔曼滤波估计
position_estimator_inav 需要解决三个问题:

  1. 如何由加速度进行速度和位置估计;

  2. 加速度偏差的计算;

  3. 如何使用融合过程。
    借用网上一个图诠释下
    PX4 代码中 position_estimator_inav(互补滤波)理解_第1张图片

    这里px4_poll()函数设定最小50Hz频率,用来保证姿态数据能够实时获取到,根据返回值判断是否符合要求,不符合要求则mavlink_log_info()函数来打印"[inav] poll error on init"信息。
    对于满足条件的情况,开始实际操作:
    1.相关数据更新
    2.检查相关主题的超时
    3.检查位置估计工具的有效性
    4.使用位置估计工具更新传感器补偿
    5.对高度和位置进行滤波处理
    6.地形估计和发布数据

先得到每个Sensor的Corr,然后进行判断是否有效–>是否采用进行弥补–> 给予多大的权重 --> 累加到一起得到–>bias
然后 加速度计值减去bias得到准确值。最后一次积分得到速度,二次积分得到位置信息。

你可能感兴趣的:(个人总结)