Diverging Local Position with EKF2 and Optical Flow

[ashwinlele]

I am using FMUK66 and px4flow+lidarlitev3 for position estimation. I have connected the FMU with USB and move the drone around with hand to check the quality of optical flow. It remains above 200 and the flow values looks good in the direction of motion. However the local_position_ned and odometry values (x and y) for this diverge quickly and abruptly reset back to zero. However if I keep the drone steady, the local position remains nearly the same. A picture of this is here: https://drive.google.com/file/d/1vW0yXY9hY4PNGQA4RiRDizbblg0CPk-I/view?usp=sharing . The z-value is accurate with LIDAR.

This is preventing me from using offboard mode with a companion computer. It would be very helpful if someone can suggest a solution to this.

Thanks in advance!

[martiasv]

Hi!

This sounds like a Kalman Filter consistency issue. Are the innovations zero-mean, and the variance of the innovations in the same order of magnitude as the innovation covariance from the filter?

A common reason for Kalman filter divergence is that the process- and measurement noises are not tuned correctly. A common statistical test for kalman filters is the Normalized Innovations Squared (NIS) test. This will give you a strong indication whether your filter is consistent or not.

See: https://cs.adelaide.edu.au/~ianr/Teaching/Estimation/LectureNotes2.pdf, page 18 under "Checking consistency", and page 21 under "Model validation" for more detail.

[bresch]

Hi @ashwinlele ,

Have you checked if the orientation is correct? If you extract the log file, you should be able to debug it using the estimator_optical_flow_vel message. Please also share it here if you need help debugging.

@martiasv EKF2 uses NIS in the innovation filter: the result is logged in estimator_innovation_test_ratio and a sample is rejected when > 1 (the test ratio is: NIS / (maximum number of std dev)^2, where the number of std dev allowed is EKF2_OF_GATE).

[Thomas000001]

hello @bresch may I ask you how do you use estimator_optical_flow_vel to adjust

[bresch]

@Thomas000001 If you fly with a GPS for example, you can compare the GPS velocity and the "flow velocity" to see if they are properly aligned. For more question, please open a post on https://discuss.px4.io