Position state estimate drifting even though point cloud information is accurate

[jafriztrillo]

I have crazyswarm setup with an OptiTrack system (PrimeX 22 and Motive 3.0.3) and have been running into issues with getting an accurate state estimate of the vehicle. Multiple crazyflie's were used to test and they were validated to have working IMUs and hovering abilities with the Bitcraze LPS. The RVIZ tracking is perfect when moving the crazyflie around manually with correct orientation and pose tracking. Checking the logs of the pointcloud topic from ROS shows it is accurately grabbing the crazyflie with pinpoint accuracy. Something happens between sending data from the pointcloud to the crazyflie itself where the state estimate drifts terribly and does not recover.

The following has been done to try and fix this issue:

1. Reflashing of the CF (STM32 & NRF) through the chooser.py
2. Rechecking the orientation and streaming parameters from Motive
3. Varying the FPS of the MoCap cameras
4. Verifying the latency rate is low (~4ms)
5. Varying the number of markers used for tracking (surprisingly a single marker works much better than multiple)
6. Using both the PID and Mellinger controller

My best guess is something went wrong during crazyswarm setup and this is a communication issue rather than an issue with the crazyflie or Motive itself. I can't seem to find any other topics on the discussion board like this. Any tips on where to move forward in tracking down this problem?

[whoenig]

This is usually caused by having not upgraded the Crazyradio firmware and thus, broadcast communication doesn't work. Note that even newly bought Crazyradio's need to upgrade to the latest official firmware, since they ship with a pretty old firmware.

To check, please enable logging of the pose and make sure that the onboard state estimate matches the motion capture data.

[jafriztrillo]

I reflashed the Crazyradio and now am receiving the correct state position however takeoff still has the same results of crashing once it gets off the ground (if it gets off the ground). The CF was also reflashed with the test version specified on the documentation page as well.

The behaviour seems to be erratic where the vehicle can hop around, flip upside, or just drift into a wall when sent a hover command with hello_world.py. This was just retested with both controllers as well.

[whoenig]

Make sure https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/launch/hover_swarm.launch#L36 is set to True if you use single-marker tracking.

[jafriztrillo]

The behaviour was seen even with logging off and only sending the position. I can get hover for about a second using both controllers before the CF goes unstable. Logging the control setpoints and state estimate makes sense for where the CF should be, so it is making me think it is still a setup issue.

Multiple drones have been retesting running firmware 2021.06, 2023.06, and 2023.07 to see if it is an issue with newer firmware causing a crash but this does not solve the issue. The last issue I can think of is the connection is through a DHCP server through a router rather than a physical connection between the OptiTrack PC and the Ubuntu system. Even though latency is low and there is a strong connection the lack of ethernet communication could cause an issue.

[whoenig]

One can measure the overall system latency by putting the crazyflie on a teeter board and logging both IMU and state estimate data. For single marker tracking having a few ms latencies shouldn't cause fatal crashes.

[jafriztrillo]

I tested this and then also did another validation test with the LocoPos nodes using crazyswarm and the CFClient. The IMU and state estimate data matched well which filters out the idea of the onboard computing isn't going well. The test with and without crazyswarm leads me to believe there is a communication issue with my crazyswarm setup. The LocoPos nodes in the CFClient give me nice hovering capabilities and control. Using the nodes in crazyswarm shows the same unstable behaviour as with the motion capture system.