Control Crazyflie with Vicon

Discussions about all things Bitcraze
Post Reply
nicuvlad94
Beginner
Posts: 28
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Control Crazyflie with Vicon

Post by nicuvlad94 »

Hello,

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')
Also, I have the "ESTIMATOR = kalman" set in the config.mk (tools/make).

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

Thank you,
Vlad
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Control Crazyflie with Vicon

Post by kristoffer »

Hi!

This i in interesting. When I wrote the qualisys python example I had some problems with the attitude that I did not fully understand. I could observe ringing (resonance) in the EKF when yaw was close to 90 (and 270) degrees, that quickly increased to a complete instability when reaching 90 degrees. It worked if I increased the locSrv.extQuatStdDev a bit and I interpreted it as that the attitude signal I used had some more noise. Maybe there is something else going on?

It could be interesting to try your static attitude experiment with a different yaw.

The implementation of the pose in the kalman estimator (in the Crazyflie) was contributed by the Crazyswarm project and as far as I know, they use it in Crazyswarm and I assume that part works. The python lib implementation was added by me, but it essentially only forwards the data to the CF.

It should work fine to use cf.extpos.send_extpos(x,y,z), but you must boot/start the Crazyflie facing the positive X direction. Attitude will be maintained using the IMU and it assumes this starting orientation. The Flow deck is not needed.
nicuvlad94
Beginner
Posts: 28
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Control Crazyflie with Vicon

Post by nicuvlad94 »

Thank you for your answer!

These are things that I already tried before I posted here. I tried different values for the std dev. Also, I statically tried to move the quad while observing the value of the yaw provided by Vicon and the value logged from the CF. So this proves that I send the correct information (and the two values were very different for any orientation)

However, I eventually implemented only the x,y,z update function. I place the CF pointing along the x-axis and I reset the EKF before takeoff. The roll/pitch/yaw tracking is really good (it perfectly matches the Vicon values).

However, one thing (possibly off-topic) is the yaw tracking when no external positioning system is used. If I turn off the Vicon data streaming to CF, so I only fly the drone using the flow deck + imu, the yaw diverges with 20 degrees in only 20 seconds. Why is the drift that crazy? And why did you choose to not use the magnetometer?
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Control Crazyflie with Vicon

Post by kristoffer »

Thanks for the information, maybe there is something fishy going on?

The kalman filter has a feature that makes the yaw "bleed" back to 0 (or actually the value set in param kalman.initialYaw) when not flying. https://github.com/bitcraze/crazyflie-f ... #L872-L879

I think it originates from the first implementation that only supported UWB positioning (that can not measure yaw).
nicuvlad94
Beginner
Posts: 28
Joined: Sun Sep 08, 2019 12:44 pm
Contact:

Re: Control Crazyflie with Vicon

Post by nicuvlad94 »

But my issue with the yaw is that it is drifting during the flight, I assume that because of integration drift (?). Would it be possible to enable the magnetometer, to have some yaw correction when the mocap is not used?
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Control Crazyflie with Vicon

Post by kristoffer »

Sorry, my bad!

That sounds a bit too much, the yaw should not drift that much with a Flow deck.
You should expect something like this https://www.bitcraze.io/documentation/t ... in-action-

If it is worse than that, please contact us as at support@bitcraze.io
Post Reply