Adding lidar measurements in the contact-aided inEKF?

[ylenianistico]

Hi, thanks a lot for this code. I am working on a multisensor state estimator for quadruped robots, and I would like to use your inEKF as a benchmark. Still, I need to add lidar measurements (position and rotation), to correct the state together with the kinematics measurements.

I'm currently defining the measurement vector as y=[y_leg, y_lid_transf], where y_lid_transf includes information on lidar rotation and position. I am not sure if this strategy will work because I'm still not obtaining good improvements, so if you have any suggestions, I will be very grateful.