Missing Visual Odometry update for quadruped robot?

[egordv]

Hello and thank you for a great repo!

In my research I'm testing the SEROW on a consumer-grade quadruped robot.
Performance with IMU/leg_odometry looks good!
But the robot drifts in vertical position, which is expected if not using the visual odometry or other bias correction.

So I need to update the estimator Kalman core with direct position measurement coming from visual system.
The problem is that the realization of updatePositionOrientation() in a quadruped mode looks copy-pasted from biped mode and then commented and abandoned.
It's not clear from the code how to rearrange the required matrices to port the biped updatePositionOrientation() to a quadruped mode.
Could you please provide some support on this topic?

[mrsp]

Hello egordv

Thanks for the feedback!
SEROW was originally designed for biped/humanoid robots where it has been highly effective and optimized for. For the latter case the vertical base position drift is very minimal.
I've added support to quadruped robots via the Invariant Extended Kalman Filter (ImuInEKFQuad). Nevertheless, this filter has not been extensively tested as the biped/humanoid filters, that's why some filter updates are commented - marked as TODO -

The safer and tested way is to follow a similar logic to the humanoid_ekf.cpp for your quadruped robot and use the ImuEKF where you can provide velocity updates from the SEROW leg odometry (DeadReckoningQuad.h) which is highly accurate
and subscribe to your visual odometry topic to update the ImuEKF base position/orientation with the updateWithOdom function with useOutlierDetection = false .