Px4-autopilot: LPE Yaw fusion issue

[FaboNo]

We are working on a quadrotor which is dedicated to the exploration of indoor environments. We have developed a SLAM and we would like to use the yaw computed by the SLAM as the trustable yaw.
Indeed as the quadrotor may fly in indoor environment with lot of metallic structures, the magnetometer may not be reliable.

Therefore, we tried to fuse the yaw coming from a SLAM with the yaw computed by the FCU.

we are using MAVROS to communicate with the FCU through the MAVROS msg /mavros/vision_pose/pose

We get the yaw value computed by FCU with the MAVROS msg : mavros/imu/data

We are aware of the fact that MAVROS provides information in ENU and thus we have to recompute the yaw computed by the SLAM in ENU as well. It is a bit tricky because :

1. The « MAP » frame of the SLAM is positioned where the slam starts, I will say ;
2. So I have to compute a Yaw offset between ENU frame and the Map/slam frame ;
3. Yaw (FCU) and yaw (SLAM) values range from -PI to +PI

On the px4 side :

ATT_EXT_HDG_M set to « vision »
Regarding LPE_FUSION :

• Fuse vision positions is checked
• Fuse vision yaw is checked

Firmware version : 1.7.3

In order to test the fusion, we put the quadrotor on a table and, pointing in a direction and performed a rotation 360 degrees. This way we can check the mag calibration and the accuracy of the yaw computed by the SLAM and the FCU.

we noticed that the fusion is not working really well since I can get usually around 20degrees difference between yaw(FCU) and yaw(slam) – the first pic shows the yaw(fcu) and yaw(slam):

The second pic shows the evolution of the delta between yaw(fcu) and yaw(slam):

Because the angles range from [-PI, +PI], if one yaw is lagging then one come up with a 350 degrees difference for a period of time (I put 0 on the graphic). How do you handle this case in practice ?

What is puzzle me as well is that most of the time, the yaw (FCU) is pointing in the right direction after the 360 degrees rotation

I created a log file as well which is available here

I do not know how to investigate in order to understand why the yaw fusion is not working...
Is it possible to tell LPE to trust more the yaw from the SLAM than the yaw from FCE ?
Is there another parameter I need to tune ?
Does somebody out there is facing the same issue ?

I update my post with the following remark: I am wondering whether it is required to be either in OFFBOARD or POSCTL mode so that LPE and the atitude control algorithm run and issue a local position? Because when i do rostopic echo /mavros/local_position/pose, nothing is published!

Thank you
Fabrice

[FaboNo]

I update my post with the following remark: I am wondering whether it is required to be either in OFFBOARD or POSCTL mode so that LPE and the atitude control algorithm run and issue a local position? Because when i do rostopic echo /mavros/local_position/pose, nothing is published!
Fabrice

[TSC21]

We are aware of the fact that MAVROS provides information in ENU and thus we have to recompute the yaw computed by the SLAM in ENU as well. It is a bit tricky because :

1. The « MAP » frame of the SLAM is positioned where the slam starts, I will say ;
2. So I have to compute a Yaw offset between ENU frame and the Map/slam frame ;
3. Yaw (FCU) and yaw (SLAM) values range from -PI to +PI

I don't quite get why you are doing this. MAVROS provides the information in the local NED frame, not ENU. You need to provide the pose of your SLAM system in ENU to MAVROS and it automatically transforms to the NED frame.

Firmware version : 1.7.3

You are using an outdated FW version in what concerns to LPE. The LPE_FUSION bitmask 3 was replaced by fuse landing target basically because fuse vision yaw wasn't doing anything: LPE handles the position estimation, not attitude, that's why it runs in parallel with attitude_estimator_q, to which you are fusing your SLAM estimated vision orientation. ATT_EXT_HDG_M is a parameter of attitude_estimator_q.

we noticed that the fusion is not working really well since I can get usually around 20degrees difference between yaw(FCU) and yaw(slam) – the first pic shows the yaw(fcu) and yaw(slam):

attitude_estimator_q is a complementary filter, so every source has a weight on the fusion. If you want to trust more on the SLAM system than on the IMU (which I actually feel a bit awkward, given that SLAM visual odometry tends to drift and it's just corrected by loop closures), you can increase ATT_W_EXT_HDG from default 0.1 to a max of 1.

Is it possible to tell LPE to trust more the yaw from the SLAM than the yaw from FCE ?

As answered, LPE doesn't estimate attitude.

Is there another parameter I need to tune ?

Answered above.

I update my post with the following remark: I am wondering whether it is required to be either in OFFBOARD or POSCTL mode so that LPE and the atitude control algorithm run and issue a local position?

After the above answer, you understand that this question does not make sense.

Because when i do rostopic echo /mavros/local_position/pose, nothing is published!

That's another issue: do you actually get a position lock when you start sending the vision estimate to the flight controller (green led fading in and fading out)? On QGC, do you get LOCAL_POSITION_NED msgs?

[FaboNo]

@TSC21 Thank very much for your detailed answer. It helps me to get a better understaning of px4/mavros - Really appreciate.

Actually after reading another thread regarding the fact the nothing was published on /mavros/local_position/pose, you talked about checking the px4_pluginlists_yaml file and check what is blacklisted. I noticed that the vision* messages where blacklisted in my px4_pluginlists_yaml. I deleted this line, and now the messages /mavros/vision_pose/pose reach the FCU (listener vehicle_vision_position in the mavlink console) and /mavros/local_position/pose is updated now. I do not know why a blacklisted message is displayed when I do a rostopic list...

However I still have a question concerning /mavros/vision_pose/pose. The coordinates provided in this message are in local ENU or local NED? It is quite important for the Z value (i.e positive or negative)?

Thank you again

[TSC21]

I answered that question on my previous comment.

[TSC21]

Closing for now. Please open a new issue if have more questions or bring them to the forum/ Slack channels.