This week I was working on a project based on crazyflie-lib-python and pyvicon (Vicon python wrapper on the Vicon DataStream SDK) which enables the crazyflie to get state infromation from the Vicon system. The idea is to have a nice framework which allows to feed in the vicon data (or from any other estimation source) as well as logging capabilities and live data plotting, all of these configurable with two or three code lines.
As a reference, I used the qualisys python example from bitcraze, using this function to send the state information: "cf.extpos.send_extpose(x, y, z, qx / ql, qy / ql, qz / ql, qw / ql)"
However, it does not work. I am testing it while also logging the state received from Crazyflie (x,y,z, roll, pitch, yaw) and live printing it in parallel with the Vicon data (x,y,z, roll, pitch, yaw). Vicon streams quaternions, but when I print them I convert to roll, pitch, yaw for more readability (same conversion relations as in the CF firmware). Logging / streaming period is 10ms or 20 ms - I tried with both.
So when I look on my data, the x, y, z logged from crazyflie perfectly match what the Vicon shows, which means that the position data successfully arrives in the estimator. However, for the attitude I see numbers which are very different from the Vicon reported angles. Also, when I fly the quad goes crazy. So I assume I have an issue with updating the attitude. I mention that the reported attitude from Vicon is aligned with the drone frame.
Also, for testing I tried to send the actual Vicon position, but instead of sending the actual quaternions I tried to send [0.5, 0.5, 0.5, 0.5] and to set the stdDev small. This would correspond to a roll = 90, pitch = 0 and yaw = 90 but I only see random flickering data...
Before flying, I perform the following config:
Code: Select all
cf0.cf.param.set_value('stabilizer.estimator', '2') cf0.cf.param.set_value('locSrv.extQuatStdDev', 0.1) cf0.cf.param.set_value('stabilizer.controller', '2') cf0.cf.param.set_value('commander.enHighLevel', '1') cf0.cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf0.cf.param.set_value('kalman.resetEstimation', '0')
Do you have any idea of what that would happen?
Some additional questions (11.05):
- I eventually used the function cf.extpos.send_extpos(x,y,z) to only send the positions (and I assume that the attitude is computed by the EKF).
- However, I would still expect the state estimate to be perfect (because perfect position estimation allows for inference to estimate the orientation). But for instance if I do not use the flow deck (so only the CF) it crashed a few times. If I add the flow deck it performs better. Is this normal? I understand that the flow deck gives the position in the body frame by integration and having the position in the world frame easily allows to calculate the rotation, but I would expect that the cross couplings in the EKF to be enough to compute the attitude with just the CF and no other decks