Page 1 of 1

go_to command python unexpected trajectory

Posted: Fri May 22, 2020 8:52 am
by chronob

i'm having trouble with the standard go_to command of the python library.
In 20-40% of the tries a simple go_to(1,0,0.8) is resulting in this behaviour:

Setup is one drone in a LPSv1 with two stations.
Code is the standard where the simple sequence is rewritten like this:

Code: Select all

def simple_sequence():
    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        with PositionHlCommander(scf) as pc:
            pc.go_to(1, 0.0,0.8)
            pc.go_to(0, 0.0,0.8)
We also tried to use the swarm example ( with one drone and the corresponding commands with the same problem.
The offset of the drone to the correct flightpath can be either to the upper right or lower right. But the distance is always the same. There is no randomness in how far away from the actual path the drone is flying.
This behaviour only occurs when flying forward or to the left. Everything is working when flying backwards or to the right.
It seems as if the LPS is failing but we checked it by placing the drone on a chair and moving it through the flying zone. The values were perfectly fine and accurate.
We tried multiple different drones, no change. Also tried different firmwareversions (including the newest of april).
When we logged the data (position stateestimate x,y and z) you can see that the drone thinks its on the right flightpath until it checks that it isn't and has to correct the path as seen in the video.

Is there something we missed or should try? Can somebody reproduce this in his/her setup?

Thanks a lot!

Re: go_to command python unexpected trajectory

Posted: Mon May 25, 2020 11:10 am
by kimberly

So from the video I can see that there seems to be an overshoot. Is this the problem that you are referring too ? That it goes to a position and then changes it last minute in order to compensate.

The go_to command creates trajectory from A (the location that is the current position of the crazyflie), to B (the abs position that you want the Crazyflie to go to). The trajectory is based on a 7th order polonomial but it does not seem that that would create such a behavior.

I actually also observe this on the lighthouse system as well so I don't think it is an LPS system issue. I'm also not sure if this a controller issue, since the trajectory should be controlled every step on the way.... But it seems like this need more investigation.

If you would do this for a larger distance, does the offset distance increase?

Re: go_to command python unexpected trajectory

Posted: Wed May 27, 2020 7:11 am
by chronob

thanks for the reply!
Yes, it does actually increase the offset distance.
I added a go_to(-0.5,0) as the first command of the with-statement.

I was looking through the docs and will try some methods to narrow the problem down:
- changing the lighthouse position method to "crossing beam" ( ... on-methods)
- changing the filter for the state estimation ( ... ary-filter)
- checking through the different ways to interact with the commander framework ( ... -lib-cflib)

I will update you if i can observe sth interesting.

Re: go_to command python unexpected trajectory

Posted: Wed May 27, 2020 8:14 am
by kimberly
So I wouldn't change the state estimation filter if I where you... The only filter that is able to handle the positioning system is only the kalman filter, which is automatically selected when the LPS/lighthouse deck is attached. The complementary filter is only suitable for manual flight or with a zranger.

What you could try to do if you would be comfortable with that, is to make an hybrid. We have heard that the angle estimation (roll pitch and yaw) tends to be better with the complementary filter than the kalman filter. You can try to run in parallel and substitute the angle estimation with the roll pitch yaw of the complementary filter. This requires a lot of hacking though and I'm not sure if it solves your problem, so its up to you if you want to go into this.

Re: go_to command python unexpected trajectory

Posted: Wed May 27, 2020 9:07 am
by chronob

i just changed the estimation method in the lighthouse_core.c file of the firmware to 0 (using the crossing beams method).
Did around 40 testflights with two different drones and had no problems.

I will do some more test flights with multiple drones but it seems to me that it's the right path.

Re: go_to command python unexpected trajectory

Posted: Wed May 27, 2020 10:59 am
by kimberly
If you tried out 0, then that is still probably using the kalmanfilter, since that is the indicator of 'anyEstimator' (see here: ... ator.h#L31). Complementary filter is 1 and the Kalmanfilter is 2.

Re: go_to command python unexpected trajectory

Posted: Wed May 27, 2020 12:16 pm
by chronob
yes, the estimationmethod i changed in the lighthouse_core.c file just changes the "sweep angle"-method to the "crossing beam" method.( ... on-methods).
so the problem is not with the ekf but somewhere in the sweep angle method i guess.
I'm sadly not skilled enough to dig any deeper into the firmware code, but it works fine for all of the drones we have now with the crossing beam method, so we'll stick to that for now.

Re: go_to command python unexpected trajectory

Posted: Thu May 28, 2020 9:53 am
by kimberly
Ah oke I guess it was just miscommunication. You also mentioned in the first post about the LPS system (which is another positioning system) but you actually mean the Lighthouse system do you?

What you can also try with the sweepangle menthod, is to just have the master basestation on ('B') and see if the same thing happens. This skip is might be due to that one base-station has a geometry estimation that does not exactly fit the other one, and that it loses sight of one and stops averaging. But if the crossbeams method is working for you you can also stick with that.