How to log position data from LPS without Kalman estimator?

All discussions related to the Loco Positioning system
Post Reply
salocinx
Beginner
Posts: 21
Joined: Wed Feb 06, 2019 7:58 pm
Contact:

How to log position data from LPS without Kalman estimator?

Post by salocinx »

Hi. Is there any raw position available in LPS so to get the position direcly bypassing the Kalman estimator? Is there any value available in the TOC before applying the Kalman estimator? If not how do you suggest I could get the direct position from LPS? I want to get the raw position to experiment with, since I seem having problems with position being messed by rotation in the Kalman estimator.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to log position data from LPS without Kalman estimator?

Post by arnaud »

The LPS measurement are not position measurement. In TWR mode the measurements are ranges to the anchor and in TDoA they are range different between two anchors. The kalman filter is the one interpreting these raw information to generate a position.

Very early in the LPS development I was using outside position estimator to get a position from the LPS data. These estimators are still in the lps-ros repos, there was a particle filer and an LMS filter. They use raw ranges data from the TWR mode. The ranges shoud have log variables. These scripts are not ready to use since they are very old, but it could be a starting point if you want to use LPS TWR measurement to estimate the position without the EKF.
salocinx
Beginner
Posts: 21
Joined: Wed Feb 06, 2019 7:58 pm
Contact:

Re: How to log position data from LPS without Kalman estimator?

Post by salocinx »

Hi Arnaud. Thanks for your reply! We depend on TDoA, since we're going to use about 20 tags (RoadRunners & CrazyFlies 2.0) simultaneously. Since we experience some issues with the LPS positioning:

A.) estimation inaccuracies while moving the tags fast -> "wrong" ellipsoid estimations
B.) sometimes sudden break-aways -> position values strive to exorbitant values (only remedy is to reset Kalman filter manually)

I would like to ask if there's any suggestion from your side how to minimize these issues. We would currently be satisified with 2D positioning if that helps. Also, as I understand the positioning system does fuse the 10DOF IMU sensor to achieve orientation and better position estimations.

Would it somehow make sense to disable the IMU fusion or set it to {0,0,0} in order to reduce the problems described above? If so, what is the easiest way to achieve, ideally without re-compiling any firmware?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to log position data from LPS without Kalman estimator?

Post by arnaud »

I think the particle filter could easily be ported to TDoA. The result would be a more accurate but more noisy position estimation, mostly with the particle filter there will be a trade-off between how accurately fast movement are tracked and how noisy the estimated position is.

my thought for your problems are:
A.) this is most likely coming from a wrong orientation estimation: if you do sudden movement, the accelerometer data will be used a lot for the short term position, if the orientation estimate is wrong this movement will be in the wrong direction. The orientation should be corrected by the kalman filter over time though so as long as you have a good initial yaW (starting facing x) this should not be such a big problem.

B.) Without knowing more about your setup it is hard to understand this one. Are you in the convex hull and does the tag have good reception on all the anchors?

This is correct that the Kalman filter is fusing 6DOF (accelerometer + gyroscope) together with the raw LPS TDoA measurements.

If you disable the IMU you might as well bypass the kalman filter. I do not think the current Kalman implementation is designed to work without IMU
salocinx
Beginner
Posts: 21
Joined: Wed Feb 06, 2019 7:58 pm
Contact:

Re: How to log position data from LPS without Kalman estimator?

Post by salocinx »

Thanks for your quick and detailed answer. I am indeed highly interested in gathering the position data without the Kalman filter, since it unfortunately seems not to meet our requirements. I understand, that it's most probably the best solution for the Crazyflie quadcopter.

We want to track people while doing sports (badminton, basketball, etc.). Therefore we can't control the movements of the tags and avoid fast movements for example.

Regarding question B.); Yes we're moving within the convex hull of the LPS anchors and line-of-sight is given to all 8 anchors at any point in time. But we're currently testing in a rather small room and LPS anchors are fixed on metallic stands. Probably not the best setup I guess ;) ?

I checked the Particle script and LMS script links. These are tightly coupled to ROS ? As mentioned before, I would be very interested to try out the Particle filter approach since you think it would be portable for TDoA. But to be honest, I don't have the slightest idea how to start.

1.) Which values shall I use for input? Distance differences to the tags from the LPS nodes? Are these values loggable through the Crazyflie/RoadRunners (which TOC entry) ?

2.) Ideally I would prefer to have the Particle filter as stand-alone Python script, since I already built quite some infrastructure to exchange data between the CF Python library and another .NET application.

Thanks for your patience and valuable help so far!
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to log position data from LPS without Kalman estimator?

Post by arnaud »

Hi,

It might help with the noise if the nodes are a bit away from the metalic structure (at least 15cm).

For your questions:
1.) The easiest is to use TDoA2 and then you have the distance difference available directly as log variables: https://github.com/bitcraze/lps-node-firmware/releases. This is the distance difference in meters between anchor couples

2.) The particle filter part should be independent from ROS: https://github.com/bitcraze/lps-ros/blo ... pfilter.py. The part to modify to make it work with TDoA is there: https://github.com/bitcraze/lps-ros/blo ... #L128-L142. Given the estimate xyz and a range meas, this function returns the likelihood this estimate correspond to the measurement.

On a side note, one thing that could help you keeping the current Crazyflie kalman filter is to remove thezero reversion: by default the kalman filter will have all angles reversing to 0 when not flying. You can switch this behavior OFF by setting ROLLPITCH_ZERO_REVERSION to 0: https://github.com/bitcraze/crazyflie-f ... man.c#L205.

You could also try to run the system in 2D only mode by running TDoA3 and recompiling the firmware with the following uncommented in tools/make/config.mk: https://github.com/bitcraze/crazyflie-f ... le#L72-L74
Post Reply