-
Notifications
You must be signed in to change notification settings - Fork 1k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
External Pose from 90' to -90' passing from 180 get crazy #1292
Comments
Hi! I have also seen instabilities around 90 degrees, I'm not sure why though. I got the feeling that the estimator is more sensitive to measurement noise in the quaternion data close to 90 degrees and I ended up increasing the standard deviation when pushing samples to it. You can set the standard deviation for extpos quaternion with he parameter |
Thanks for the reply As a first solution (bypass of the problem), i disabled in "estimator_kalman.c" all the sensors but KalmanUpdateWithPose and it worked as supposed (apart from some strange noise on the Z axis but is something that is not very annoying), starting from that i started to include sensors one at time but i still didn't completed the research to find which sensor or which bug create this situation. I will update you soon, in the meantime i will study better how the stddev works for the kalman filter and if there is a way to let it work without doing strange stuff. Thank you |
Another possibility is to only push in the position (not full pose), the drawback is that this requires that you start the Crazyflie facing the positive X direction (or tell the kalman estimator what the current yaw is just before takeoff). Roll, pitch and yaw will be estimated from the IMU and motion of the drone and works pretty well also. If you want to try this, you can find a function in the python lib, or message in the Crazyflie (depending how your system works) that only takes position but no attitude. |
could be cool but actually I'm not using a crezyflie but i created a blimp with position control and the main problem that I'm facing is the yaw drift due to the inertia. paradoxically i would prefer a working yaw that a working x y and z. Thanks for the reply |
I see. Sounds like a fun project! By the way, if the problem actually is noise in the attitude data, it is also possible to add filters in the QTM software to change the output of the Qualisys system. |
The problem is very systematic, I'm not using qualisys right now but I created a little script python that simulate qualisys data. and with raw perfect data it gives me a really specific error. I was working on a solution in these days and I think that I found the problem and maybe also the solution (not the best one). The problem is in the mm.pose.c file. In particular I think that there is a problem with the calculation of the error using the small angle approximation. I tried to find the precise problem but I failed. So I decided to try a workaround and it worked. crazyflie-firmware/src/modules/src/kalman_core/mm_pose.c Lines 40 to 46 in 2cf04d7
from this line instead of use the quaternion approximation and yada yada, I converted into RPY and did the difference between the ekf and measured. As I said this is not a real solution but at least I can work properly with the yaw.
In this way it works (almost) perfectly (for my project). In the future maybe i could find a real solution to this bug. Thank you for the support Obv these are all my guesses maybe the problem is somewhere else. |
Interesting! |
It would be good to get a numerical example when the code fails. If the small angle approximation is indeed to blame, you can simply replace it with The RPY idea is bad, because the difference of angles can be very wrong (i.e., the line |
I did some other test in the firmware to find where the error could be possible, and after many hours I didn't find what is not working but at least i excluded what for sure is working. eq0 0.800285, eq1 0.000003, eq2 0.000020, eq3 0.599619
mq0 0.799160, mq1 0.000000, mq2 0.000000, mq3 0.601117
eq0 0.789999, eq1 -0.000018, eq2 -0.000022, eq3 0.613107
mq0 0.788547, mq1 0.000000, mq2 0.000000, mq3 0.614973
eq0 0.778868, eq1 0.000019, eq2 -0.000018, eq3 0.627187
mq0 0.777694, mq1 0.000000, mq2 0.000000, mq3 0.628642
eq0 0.768139, eq1 -0.000010, eq2 0.000037, eq3 0.640282
mq0 0.766605, mq1 0.000000, mq2 0.000000, mq3 0.642118
eq0 0.756887, eq1 -0.000025, eq2 -0.000118, eq3 0.653544
mq0 0.755281, mq1 0.000000, mq2 0.000000, mq3 0.655400
eq0 0.744992, eq1 0.000294, eq2 0.000327, eq3 0.667072
mq0 0.743728, mq1 0.000000, mq2 0.000000, mq3 0.668481
eq0 0.733365, eq1 -0.001752, eq2 -0.000939, eq3 0.679831
mq0 0.731948, mq1 0.000000, mq2 0.000000, mq3 0.681359
eq0 0.721209, eq1 0.012111, eq2 0.002608, eq3 0.692606
mq0 0.719945, mq1 0.000000, mq2 0.000000, mq3 0.694030
eq0 0.710040, eq1 -0.101721, eq2 -0.003689, eq3 0.696764
mq0 0.707723, mq1 0.000000, mq2 0.000000, mq3 0.706489
eq0 0.713985, eq1 0.233391, eq2 -0.292064, eq3 0.591989
mq0 0.695285, mq1 0.000000, mq2 0.000000, mq3 0.718733
eq0 0.744789, eq1 0.382161, eq2 0.082239, eq3 0.540813
mq0 0.682636, mq1 0.000000, mq2 0.000000, mq3 0.730758
eq0 0.748089, eq1 0.166421, eq2 0.404852, eq3 0.498758
mq0 0.669778, mq1 0.000000, mq2 0.000000, mq3 0.742560
eq0 0.702521, eq1 -0.113826, eq2 0.420751, eq3 0.562561
mq0 0.656717, mq1 0.000000, mq2 0.000000, mq3 0.754136
eq0 0.684654, eq1 -0.280628, eq2 0.330647, eq3 0.585805
mq0 0.643455, mq1 0.000000, mq2 0.000000, mq3 0.765483
eq0 0.685283, eq1 -0.374943, eq2 0.223757, eq3 0.582868
mq0 0.629998, mq1 0.000000, mq2 0.000000, mq3 0.776596
eq0 0.699936, eq1 -0.433852, eq2 0.101051, eq3 0.558256
mq0 0.616348, mq1 0.000000, mq2 0.000000, mq3 0.787473 Obviously is something trivial but doing other tests with stdDev to 1.0e-1 i found that this noise reduces a lot (but everytime the device moves a little bit it doesn't trust anymore the pose) and the state estimation is not accurate. At this point i think that the problem is in the kalmancoreScalarUpdate but i don't know if is just a limit of the kalman filter itself or of the implementation. Thank you for all the support, if you have some idea on how to proceed or if you need some tests to confirm some hypothesis, it will be a pleasure to help |
Unfortunately I don't have that much to add at this point. |
I'm working on it, I found some critical parts and the performance are already improved a lot compared to the master, I think that is something that could be fixed, I will update you soon when i find enough to talk about it |
I don't think I have time anytime soon to dive into this, but my approach would be to collect event data on the uSD card and then use the Python bindings to analyze/debug the EKF on the PC. |
Hi, i'm doing a project that involves the use of qualisys to feed the position but I found a problem that are days that I'm trying to solve.
Feeding with extpose, using quaternions works pretty good except for a case. When my device goes from 90 degree to 180 degree and from -180 to -90 degree the state estimation doesn't work anymore.
I have a picture of the output of qualysis and the output of the state estimation. (don't mind if one is in rad and one is in deg with opposite sign)
![Screenshot from 2023-06-07 15-55-19](https://private-user-images.githubusercontent.com/119309168/244049535-8d44d600-a13a-4a7f-8b29-484c17b95af3.png?jwt=eyJhbGciOiJIUzI1NiIsInR5cCI6IkpXVCJ9.eyJpc3MiOiJnaXRodWIuY29tIiwiYXVkIjoicmF3LmdpdGh1YnVzZXJjb250ZW50LmNvbSIsImtleSI6ImtleTUiLCJleHAiOjE3MjU5NDI5NDcsIm5iZiI6MTcyNTk0MjY0NywicGF0aCI6Ii8xMTkzMDkxNjgvMjQ0MDQ5NTM1LThkNDRkNjAwLWExM2EtNGE3Zi04YjI5LTQ4NGMxN2I5NWFmMy5wbmc_WC1BbXotQWxnb3JpdGhtPUFXUzQtSE1BQy1TSEEyNTYmWC1BbXotQ3JlZGVudGlhbD1BS0lBVkNPRFlMU0E1M1BRSzRaQSUyRjIwMjQwOTEwJTJGdXMtZWFzdC0xJTJGczMlMkZhd3M0X3JlcXVlc3QmWC1BbXotRGF0ZT0yMDI0MDkxMFQwNDMwNDdaJlgtQW16LUV4cGlyZXM9MzAwJlgtQW16LVNpZ25hdHVyZT01ZjA1ZTNhN2IzZmViNjQ0NmU4NThmZmNkYjJhOTM0ZTMzYWNmMWY2MjhjYTRkNzI4YWUzMTZmMDc5MzJhNzRlJlgtQW16LVNpZ25lZEhlYWRlcnM9aG9zdCZhY3Rvcl9pZD0wJmtleV9pZD0wJnJlcG9faWQ9MCJ9.oJfGDbNbyBGd8nDEqWAMjbK4kLrTXLfgAzTnAkk08P4)
The text was updated successfully, but these errors were encountered: