I tried another way to implement this. Currently my flight core is disconnected to the VOXL. I use my laptop as the companion computer, and then I add a ESP Wifi module to Flight Core TELEM1, so my laptop will get the motion capture pose by using vrpn_client_ros, the mavros will transfer the motion capture pose to mavlink data and then send to flight core.
In this case, my EKF2 will converge. The new issue is that the modalAI cannot hover at it's height on flight mode or hold mode, and the modalAI will drift. Also, the mavros terminal will show:"RTT too high for timesync: 78 ms". I am not sure if this is the reason that the modalAI will drifting in position mode.
Any suggestion?