Local_position_estimator 为卡尔曼滤波估计
position_estimator_inav 需要解决三个问题:
-
如何由加速度进行速度和位置估计;
-
加速度偏差的计算;
-
如何使用融合过程。
借用网上一个图诠释下
这里px4_poll()函数设定最小50Hz频率,用来保证姿态数据能够实时获取到,根据返回值判断是否符合要求,不符合要求则mavlink_log_info()函数来打印"[inav] poll error on init"信息。
对于满足条件的情况,开始实际操作:
1.相关数据更新
2.检查相关主题的超时
3.检查位置估计工具的有效性
4.使用位置估计工具更新传感器补偿
5.对高度和位置进行滤波处理
6.地形估计和发布数据
先得到每个Sensor的Corr,然后进行判断是否有效–>是否采用进行弥补–> 给予多大的权重 --> 累加到一起得到–>bias
然后 加速度计值减去bias得到准确值。最后一次积分得到速度,二次积分得到位置信息。