weixin_39830020
weixin_39830020
2021-01-10 03:57

LPE fusion jump in flight 1.6RC1 (nuttx-px4fmu-v2-lpe.px4) with external vision pose and optical flow

Hi, I am using the 1.6RC1 (nuttx-px4fmu-v2-lpe.px4) firmware. I was using SVO with visual-inertial fusion and px4flow together. The LPE fusion was set to fuse optical flow, fuse vision position, fuse vision yaw, fuse land detector, pub agl as lpos down, fuse flow gyro compensation. ATT_EXT_HDG_M was set to vision.

I think somehow the lpe was reseted during flight, almost crashed on the net. jump

I am struggling to reset the lpe on the ground. The SVO have different origin everytime it reinitialises. Any recomentation on how to do this properly?

该提问来源于开源项目:PX4/Firmware

  • 点赞
  • 写回答
  • 关注问题
  • 收藏
  • 复制链接分享
  • 邀请回答

8条回答

  • weixin_39606137 weixin_39606137 3月前

    Your vision measurements have something wrong with them - they keep resetting to zero.

    This might also be a bug I introduced by mistake. Let me check.

    点赞 评论 复制链接分享
  • weixin_39830020 weixin_39830020 3月前

    Thanks for your reply , I hard limited the update rate of my package to send vision_pose at 7Hz to mavros. There is no way it could send 14 measurements per second (7 measurements and 7 zeros). It was normal when live streaming from pixhawk to QGC, but the logging from SD card is always like this. I suspect these zeros were introduced in the logging to SD card.

    Is there a way I can manually reset the LPE apart from power cycling the pixhawk?

    点赞 评论 复制链接分享
  • weixin_39606137 weixin_39606137 3月前

    Ah, okay gotcha. It's how the topics update in sdlog2 now. I changed it so now there are 2 topics, vision_attitude and vision_position, so when the attitude topic updates, position is zero'ed. I don't see a point in 'fixing' it now. If you move to the new logger, it will be fine. :)

    And no, we don't have a way to reset LPE. You will need to write the code yourself in order to reset it when SVO resets.

    点赞 评论 复制链接分享
  • weixin_39830020 weixin_39830020 3月前

    OK, I see. Thanks
    Or if the vision bias was computed after the time out as stated in https://github.com/PX4/Firmware/issues/5478 That maybe a better way to deal with the vision odometry reset.

    I guess this was what happend before 24s in the plot. However, in this case, I don't understand what can possiblly cause the lpe reset at 24s?

    点赞 评论 复制链接分享
  • weixin_39610807 weixin_39610807 3月前

    It looks like it was considering vision faulted and then when it got close enough to the estimate and the estimate covariance got large enough, it considered it OK and started correcting to the vision position. Look at the messages sent to QGC. It should tell you when you get faults. With visual odometry, you might want to consider sending the vision velocity measurement instead of the vision position estimate. This will avoid the issues with the origin.

    点赞 评论 复制链接分享
  • weixin_39830020 weixin_39830020 3月前

    Interesting. Thanks . Yes, the 'vision faulted' and 'vision OK' message was popping out from time to time, which must be the problem then. Looks like it is better to use vision velocity instead.

    For this one, I want to clarify 3 things: 1. if the vision bias was computed after vision timeout in the 1.6RC1 LPE firmware? (there was a vision timeout at 7s-9s in the plot, so the bias should be computed. 2. Does the 'vision faulted' means the vision measurement will not be used in the LPE, and the bias will be zeroed? 3. what is the criteria for determining the vision fault? is it just checking the vision measurement (subtracted the bias) lies in the state covariance?

    点赞 评论 复制链接分享
  • weixin_39610807 weixin_39610807 3月前
    1. There is no-longer any vision bias term. It is assumed the vision position measurement error is a zero mean gaussian.
    2. Yes, if faulted, the vision correction doesn't happen.
    3. The condition is based on a chi squared test with the innovation covariance (HPH^T +R ) and the error. The false alarm rate is set to 1 in 1000.
    点赞 评论 复制链接分享
  • weixin_39830020 weixin_39830020 3月前

    OK, I see. Thanks for the clarification.

    点赞 评论 复制链接分享