UAV instability during trajectory tracking using CTU-MRS with mrs_point_lio state estimator

[MJavadZallaghi]

Dear MRS team, @klaxalk, and community,

I have a UAV running the CTU-MRS autonomy stack. I’m using the mrs_point_lio state estimator, the SE(3) Controller, and the MPC Tracker.

For indoor trajectory tracking, I designed a simple rectangular path. The UAV tracks it successfully when the heading is fixed at 0° or when the yaw-rate is very low. However, the vehicle becomes unstable mid-trajectory when the desired heading changes more quickly.

To illustrate this, I ran four tests on the same path with different desired-heading profiles. For each test, I recorded camera video, RViz, and the heading signals (from the state estimator, the control reference, and the control error). These logs show how the UAV’s stability degrades as the heading change rate increases.

Test 1: Test 355

• Analysis video: ​20251103_120735_1_Javad_X650_test_355_edited.mp4​
• Controller and Tracker: MPC Controller of MRS + MPC Tracker of MRS [Also SE3 Controller made the same result, but the bag file of the flight with that controller wasn't recorded correctly].
• Trajectory: Rectangle path (22 points for an area of 4.2 m × 3 m) with 0 deg heading on the first line, 90 deg heading on the second line, and 180 deg heading on the third line
• Result: Failed
• Analysis: After tracking the first line and performing the first 90-degree rotation, the UAV started to be unstable. The heading plot shows that the estimation of heading is consistent with the real-world data (from the camera and RViz).

Test 2: Test 361

• Analysis video: ​20251104_192210_3_Javad_X650_test_361_edited.mp4​
• Controller and Tracker: SE3 Controller of MRS + MPC Tracker of MRS
• Trajectory: Rectangle path (22 points for an area of 4.2 m × 3 m) with 0 deg heading on all the lines
• Result: Success
• Analysis: The tracking here is perfect, no instability, and heading estimation is consistent with the real-world (from camera and RViz) value.

Test 3: Test 363

• Analysis video: ​20251105_182520_1_Javad_X650_test_363_edited.mp4​
  Controller and Tracker: SE3 Controller of MRS + MPC Tracker of MRS
• Trajectory: Rectangle path (22 points for an area of 4.2 m × 3 m) with increased heading of 0.05 rad/point
• Result: Success
• Analysis: The tracking here is good but not perfect (especially at the end), with no instability. The heading estimation is consistent with the real-world value (as observed from the camera and RViz).

Test 4: Test 364

• Analysis video: ​20251105_182520_2_Javad_X650_test_364_edited.mp4​
• Controller and Tracker: SE3 Controller of MRS + MPC Tracker of MRS
• Trajectory: Rectangle path (22 points for an area of 4.2 m × 3 m) with increased heading of 0.10 rad/point
• Result: Failed
• Analysis: The instability starts after reaching the start of the third line of the trajectory. This test demonstrates that even when the UAV was instable, the heading estimation remained consistent with the real-world value (as observed from the camera and RViz). If we replay seconds 00:45 to 00:54 and compare the heading of the UAV frame with the logged value, we can see the estimation was correct even when the UAV was moving in an instable situation.

I need to have stable flights so I can go and implement my research :D ; So really appreciate it if you can help me or give insights what cause this instability problem when trying to track a trajectory that has a variable heading path.

I can upload the bag files for all the tests and share them.

[matemat13]

Hmm on the one hand, if the estimation outputs a correct heading then it should be a control problem and not an estimation problem. I see you are using the supersoft gains for the SE(3) - I'm not sure if those are the most suitable for your platform. Also, I would recommend trying the MPC controller - it is less aggressive and more robust than SE(3), so it may lead to more stable flight.

On the other hand, the behaviour I see in the videos seems clearly like "whirpooling", which is usually caused by a bad heading estimation... so maybe the Point-LIO is the problem after all! Can you check the output of the Point-LIO if it's maybe complaining about something?

BTW can you replicate this when flying on GPS? Do you by some chance have access to some ground-truth data (such as motion capture) to validate if the heading estimation is correct or not?

[MJavadZallaghi]

Hi @matemat13, thanks for checking my issue!

Hmm on the one hand, if the estimation outputs a correct heading then it should be a control problem and not an estimation problem. I see you are using the supersoft gains for the SE(3) - I'm not sure if those are the most suitable for your platform. Also, I would recommend trying the MPC controller - it is less aggressive and more robust than SE(3), so it may lead to more stable flight.

I tried first with MPC controller - test 1 (355) used MPC controller, and the problem of the instability exists even with MPC.

On the other hand, the behaviour I see in the videos seems clearly like "whirpooling", which is usually caused by a bad heading estimation... so maybe the Point-LIO is the problem after all! Can you check the output of the Point-LIO if it's maybe complaining about something?

Yes I will check the terminal log of the point-lio and will share it here tomorrow, but in RViz of all the tests even failed ones (Test 1 and 4) I think what I see as heading estimated by the state estimator is consistent with real-world heading of the UAV from camera view. Isn't it?

BTW can you replicate this when flying on GPS? Do you by some chance have access to some ground-truth data (such as motion capture) to validate if the heading estimation is correct or not?

We don't have a mocap room; I should follow up about this; And about outdoor flight with GPS, I have not tested it yet.

There is one thing that I'm interested in to ask from you @matemat13 and @klaxalk. The UAV I'm using is based on Holybro X650 dev kit. This kit has four Tmotor MN4014 KV330 Motor with 1555 props [datasheet from manufacturer here]. I'm using the T650 platform model params of MRS for performing my flights with mrs system. Do you think model parameter mismatch can be the root cause of instability?

[matemat13]

If you're using different motors or propellers, then mismatch of the thrust curve model can definitely be a problem, although the behavior in that case would probably be different (lazy/too fast takeoff, unstable height control, etc.). But it is definitely something to check. I would recommend trying to calibrate your thrust curve.

Flying with GPS would also be top on my list of tests - to see if the problem persists also with a more reliable and better tested state estimator then Point-LIO.

As with any SLAM/VIO/LIO, Point-LIO can fail in some cases, such as when there are insufficient features in the environment or some other assumptions about the environment are not met. For example, one thing I can see from the videos is that one of the walls is some kind of a curtain that is strongly deforming under the wind gusts from the UAV, which breaks the assumption of most such algorithms about the environment being mostly static. Although this doesn't seem to be the cause of your problems, it could degrade the performance and it would be one of the things I'd try to rule out. Other problems when using SLAM/VIO/LIO can be slow computation and/or too large delay in the output data, which can also destabilize the control.

Good luck and let us know when you have some more data :) Btw. from which university are you?

[MJavadZallaghi]

Thanks for all the suggestions!

If you're using different motors or propellers, then mismatch of the thrust curve model can definitely be a problem, although the behavior in that case would probably be different (lazy/too fast takeoff, unstable height control, etc.). But it is definitely something to check. I would recommend trying to calibrate your thrust curve.

Yeah, I agree. Do you recommend any procedure for finding the thrust curve? I don't have a standard stand for that at the moment, but I thought maybe I could put some weights on the UAV, perform hovering with MRS after takeoff, and see the motor RPMs from the PX4 logs. So, using four different weights, I can probably find an estimation. The datasheet from the provider has limited data points, and I'm unsure if they have exaggerated the information or not.

Good luck and let us know when you have some more data :) Btw. from which university are you?

I'm Javad, a second-year PhD student at Vrije Universiteit Brussel (VUB) and BruBotics research group, doing research on learning-based planning and navigation for aerial robots. I attended the Prague MRS summer school in 2025 this year; maybe we met there and just can't recognize each other from our GitHub accounts. :D

More data logs from the tests:

These are the bags of the shared tests. The password to the files is bags_javad_x650. Also these are terminal logs of point_lio:
Test1_355_SLAM.log
Test2_361_SLAM.log
Test3_363_SLAM.log
Test4_364_SLAM.log
And core.log files:
Test1_355_Core.log
Test2_361_Core.log
Test3_363_Core.log
Test4_364_Core.log

I didn't see that in the core.log that the state estimator is complaining about something seriously. Do you see any important warnings or information?

Thanks for all the hints and guides!

[matemat13]

Regarding the thrust curve, we have a tutorial on that, see https://ctu-mrs.github.io/docs/1.5.0/api/motor_thrust_model. The most accessible method is as you suggested - changing the payload mass and measuring the throttle/thrust relationship.

I'll try to find some time to take a look at the logs.

[MJavadZallaghi]

Hi @matemat13
I fitted my thrust curve based on MRS tutorial. I documented my flight logs for six different total payloads here. The previous params that I was using are a: 0.32 b: -0.437, and after thrust-curve fitting, new params are a: 0.342745 b: -0.464607, so just a little different.

I continued to test trajectory tracking with new thrust-curve params, and I tested the trajectory with two 90-degree turns. Still, the instability problem exists, but it seems to happen a little after the previous flight. This is camera-view compare of the same trajectory tracking with the SE3 controller, with the old thrust curve (test 355) and the new thrust curve (test 369). Do you see it as a slight improvement in tracking compared to the previous one?

I was suspicious that the point-lio state estimator becomes unreliable in the area where instability starts. To check this, I flipped the flight start point to another corner of the room. The instability also happened in this flipped-trajectory test. This test is named 371, and I have made a comparison video with test 369 for it. Therefore, the problem is not with that part of the test site, based on this test (371).

I would appreciate it if you could check these videos and, if you have any extra hints or debugging steps for me. Thanks in advance for your time and consideration.