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

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.

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?

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.

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?

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.

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?

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.

OK, I see. Thanks for the clarification.