We are using a 3D Lidar to generate position estimates in indoor environments on a Jetson TX2 and then feed these into the flight controller using the VISION_POSITION_ESTIMATE. We are using a CubeOrange with 4.0.5, additionally, we have an Optical Flow sensor and Laser Altimeter. With a previous SLAM method we were testing we had oscillations on all axis and ultimately it seemed to be a problem with our update rate, 2-4hz, as well as the latency of the sensor data(700-800 ms). Using a different SLAM method we were able to solve these issues, as it integrates IMU data into the position estimate allowing for a faster refresh rate(~30hz) and latency close to (100ms). After this change, we flew the quad in both position hold and loiter and it was noticeably more stable specifically in the Z-axis which was previously very noisy. However, there were still significant oscillations in positioning specifically when hovering however it seemed to stay around the desired point. Increasing the EK2_POSNE_M_NSE from 0.1to 0.5 seemed to smooth these movements significantly and while the oscillations still existed it was comparable to drifting compared to the active adjustments before. This, however, is still not precise enough for our applications and my current hunch is that it has to do with the tuning of the Ardupilot Position controller. Might anyone have an idea of what could be going on in our system?
LOG FILES: https://drive.google.com/drive/folders/1JwaO_fbJlj4rLNO3GlOJKcKqPtiZ1OdO?usp=sharing
There are 5 sets of log files In the following google drive link, only log_27 and log_109 have images associated with them.
LOG 27
This is the only flight in guided mode, the drone is in Guided mode and is given a take off message and given a command to move 2m in the Y and then back to land. There is a hold for ~30sec in between each move. The rotate is caused by the way we are setting the EKF origin and is not part of the command. We observed that it seemed to hover around the correct points as well as landed in the proper position leading us to believe it had more to do with PIDs rather than sensor drift.
EK2_POSNE_M_NSE=0.3
LOG_105
This flight was in Position hold and had the most aggressive oscilations in hover.
EK2_POSNE_M_NSE=0.1
LOG_107
This flight was in Loiter
EK2_POSNE_M_NSE=0.5
LOG_108
This flight was in position hold and was much more stable than loiter but had a large slow oscilation.
EK2_POSNE_M_NSE=0.5
LOG_109
To reduce the drift I chose a middle ground for the error which seemed to maintain the large slow oscilation but might have been a little tighter. This flight has a video so you can see for yourself.
EK2_POSNE_M_NSE=0.3
Conclusion
These tests were run to see the effects of EK2_POSNE_M_NSE on our positioning accuracy and performance but while it does seem to smooth out the error it does not solve the root cause of our positioning issue. We are currently performing more tests to tune the position control PID values and I will post updated logs for those. However in order to get ahead of the curve, does it look like I misdiagnosed the problem?