StdDev of External Measurements

Discussions related to positioning systems where the position is calculated outside the Crazyflie and sent to the device using radio, including MoCap systems
Post Reply
Naidala
Beginner
Posts: 11
Joined: Fri Dec 11, 2020 9:10 am

StdDev of External Measurements

Post by Naidala »

Hello. I have some questions about noise and CF kalman filtering:
  • Is there a simple way to know the default values of extQuatStdDev (and extPosStdDev)?
  • Is it possible to know the standard dev the Flowdeck has? In order to compare it with the one for external measurements (such as Vicon) and see which one is the more preferred.
  • How much confidence the KF has on measurements and how much on the model? In other words, what is the process covariance?
I would appreciate your help very much, thanks!
whoenig
Expert
Posts: 377
Joined: Mon Oct 27, 2014 2:55 am

Re: StdDev of External Measurements

Post by whoenig »

1. Yes, https://github.com/bitcraze/crazyflie-f ... #L115-L116. You can also query the current values of locSrv.extPosStdDev and locSrv.extQuatStdDev using the parameter subsystem.
2. There is an adaptive mode, where the stddev is based on the actual data from the sensor. However, by default it is fixed and you can find the value in https://github.com/bitcraze/crazyflie-f ... v1v2.c#L79.
3. The process noise default values are defined here: https://github.com/bitcraze/crazyflie-f ... #L131-L135.

Note that all those values are tuning variables and are changed in some settings. For example, the Crazyswarm has custom filter gains when using a mocap, e.g., https://github.com/USC-ACTLab/crazyswar ... ml#L64-L68.
Naidala
Beginner
Posts: 11
Joined: Fri Dec 11, 2020 9:10 am

Re: StdDev of External Measurements

Post by Naidala »

Thanks a lot! That was precious!
I also added a "wait_for_position_estimator" to let the Kalman converge, and it works correctly (except on Z, but probably we accidentally covered the flowdeck).
photo5987965820250207776.jpg
I just have one more question: when we removed the flowdeck and let CF take external measurements from Vicon only, it did not work. Actually, it didn't even take off, since values in wait_for_position_estimator were not converging under the threshold.
I think we set Kalman with the following lines, AFTER the logging configuration (adding the variables to Log and the callback, and logconf.start()): should it be put BEFORE all of that?

Code: Select all

        
        cf.param.set_value('stabilizer.estimator', 2)
        cf.param.set_value('kalman.resetEstimation', '1')
        time.sleep(0.1)
        cf.param.set_value('kalman.resetEstimation', '0')
Thanks again!
whoenig
Expert
Posts: 377
Joined: Mon Oct 27, 2014 2:55 am

Re: StdDev of External Measurements

Post by whoenig »

Glad you got it working so far!

The order does not matter - logging variables will remain independent on what estimator you choose and you can switch estimators at any time. How do you push external measurements from Vicon to the Crazyflie? Do you send positions or full pose information? To me, it sounds like the Kalman filter might not receive those external measurements and that's why it doesn't converge.

A side note:

Code: Select all

cf.param.set_value('kalman.resetEstimation', '0')
is not needed and might actually hurt you, since set_value is not blocking. The firmware sets this parameter automatically back to 0 after it handled the request. If you copied that from some official example, we should fix it there.
Naidala
Beginner
Posts: 11
Joined: Fri Dec 11, 2020 9:10 am

Re: StdDev of External Measurements

Post by Naidala »

Yes, I want to send the full pose! So, maybe Kalman does not receive those measurements and I'm trying to figure out why.
Just to be sure, the max size of a CRTP packet payload is 30 bytes, right?
But now a question comes to mind.
In C, a float takes only 4 bytes, thus I should be able to send (3positions+4quaternion components)*4bytes = 28 < 30 bytes.
But in Python a single float is treated as an object and consists of 24 bytes.
Is this different-size situation taken into account in crazyflie-lib-python, when building a CRTPPacket() the firmware will receive?
kristoffer
Bitcraze
Posts: 477
Joined: Tue Jun 30, 2015 7:47 am

Re: StdDev of External Measurements

Post by kristoffer »

If you use the send_extpose() method in the Extpos class (https://github.com/bitcraze/crazyflie-l ... pos.py#L54), the execution ends up in
https://github.com/bitcraze/crazyflie-l ... #L145-L159 where the pose is encoded into 4 byte floats, so the full pose fits into one packet.
Post Reply