How to log bare DWM1000 positioning data?

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 bare DWM1000 positioning data?

Post by salocinx »

Hi!

As I understand, the Kalman filter is for sensor fusion to achieve more accurate positioning results (IMU/AHRS+Positioning Data).

I am currently trying to log the DWM1000 positioning data before it gets into the Kalman filter, because I need the "raw" positioning data. For this purpose I introduced a new logging block as follows (estimator_kalman.c):

Code: Select all

LOG_GROUP_START(dwm1000_raw)
  LOG_ADD(LOG_FLOAT, positionX, &coreData.B[0])
  LOG_ADD(LOG_FLOAT, positionY, &coreData.B[1])
  LOG_ADD(LOG_FLOAT, positionZ, &coreData.B[2])
LOG_GROUP_STOP(dwm1000_raw)
This works fine and is displayed as new log block option in the log tab in the PC-Client application. Moreover I extended the kalmanCoreData_t structure (kalman_core.h) with a float array, called B[] where I plan to put the raw DWM1000 positioning data into.

I initialize the B[] array with 0.0 in kalmanCoreInit() function at kalman_core.c. This function is called successfully, tested with my ST-Link/v2 debugger and also with some initial values other 0.0 and it displayed fine in the PC-Client.

However, I tried many different functions to assign the DWM1000 raw positioning values to the B[] array; such as:

https://github.com/bitcraze/crazyflie-f ... man.c#L542
https://github.com/bitcraze/crazyflie-f ... ore.c#L319
https://github.com/bitcraze/crazyflie-f ... tor.c#L159

Unfortunately to no avail. These functions seem not to be called at all; although my LocoPositioning setup works fine, since I get positioning data in the PC-Client's LocoPos tab.

Any hints / ideas at which code line I could grab the raw DMW1000 positioning data?
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: How to log bare DWM1000 positioning data?

Post by arnaud »

The problem here is that there is no such things as "raw DMW1000 positioning data" in the Crazyflie, not in the shape of an X,Y,Z position at least.

In Crazyflie, the UWB positioning system has two modes, Two Way Ranging (TWR) and Time Difference of Arrival (TDoA). Measurements from each modes are pushed directly in the kalman filter (EKF).

In TWR mode, the raw measurements are distance to anchors which, for each anchor, puts the Crazyflie on a sphere around the anchor. The data pushed in the EKF in that case is the distance to a point in space (the anchor): https://github.com/bitcraze/crazyflie-f ... #L366-L395.

In TDoA mode, the raw measurement is the difference of distance between two anchors, this puts the Crazyflie on a paraboloid in space. The data pushed in the EKF is the difference of distance between two point in space: https://github.com/bitcraze/crazyflie-f ... #L398-L459.

For TWR there is an existing mechanism that can stream every ranges measured by the LPS deck back to the PC: https://github.com/bitcraze/crazyflie-f ... ice.c#L223. This function is called by the LPS TWR code each time a range to an anchor is measured.There is a function in the python used to receive the data: https://github.com/bitcraze/crazyflie-l ... ion.py#L99.
salocinx
Beginner
Posts: 21
Joined: Wed Feb 06, 2019 7:58 pm
Contact:

Re: How to log bare DWM1000 positioning data?

Post by salocinx »

Thanks for replying and the additional information. I am relying on TDoA, unfortunately TWR is due to its contraints no option for my application(s).

If I understand you and the kalman_core.c code correctly, the absolute position within the world frame is actually never calculated directly (on the basis of the TDoA values). The Kalman filter is only updated with the differences in movements from time-step to time-step.

My goal, especially with the RoadRunner, is to use it with other (ground) robotic vehicles and perhaps even to track people doing sports or other activities. Therefore the modeled equations for drone specific movements currently implemented in the Kalman filter do not hold up for my purpose. I guess that's the reason why I get some strange positioning values when moving the RoadRunner unlike a drone.

Do you have any suggestions/pointer on how to implement an appropriate algorithm to calculate the absolute position on the basis of the TDoA values only (without incorporating the drone model/equations) ?
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: How to log bare DWM1000 positioning data?

Post by kristoffer »

Hi!

Yes, you are correct. The position estimation is done in the kalman filter, it is never calculated directly.
the modeled equations for drone specific movements currently implemented in the Kalman filter do not hold up for my purpose
This is also correct, and is something that we would love to fix.

I think you have two basic options:
1. Write your own multilateration algorithm (https://en.wikipedia.org/wiki/Multilateration)
2. Adapt/rewrite the kalman filter to your needs

I think 2. is preferable if possible. The state used in the kalman filer could probably be simplified to position + velocity in your case, and the motion model should be fairly simple as well. Some code can be reused (measurement models) while state update and so on has to be modified. I have created a github issue for this (https://github.com/bitcraze/crazyflie-f ... issues/542) for further discussions if you are interested in contributing to the code base.
pimpam
Beginner
Posts: 19
Joined: Fri Aug 07, 2020 8:07 am

Re: How to log bare DWM1000 positioning data?

Post by pimpam »

Hello guys!

What if I use Crazy Bolt attached with the loco positioning tag and use the TDOA mode, can I do it in the same way? I mean "modify the code on "kalman_core.c" https://github.com/bitcraze/crazyflie-f ... #L398-L459 and flash new firmware to Crazy Bolt.
Is there any other things I have to do more.

It would be better if you can give me more explanation or point out which part of the code I have to modify. I'm very new with embedded systems or firmware.

P.S. In my application, I want to use the Crazy Bolt attached on the wrist of human. And when the subject performs some simple exercising movements, I can only get the x, y, z position from the LPS without any information from other sensors like IMU.
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: How to log bare DWM1000 positioning data?

Post by kristoffer »

I think it should work. The Bolt uses the same firmware as the Crazyflie 2.1 so no special handling is required.

There is some refactoring going on for the kalman filter at the moment and I think it is a good idea for you to wait for them to be done, hopefully within a week or so (https://github.com/bitcraze/crazyflie-f ... issues/722). Currently the IMU data is handled differently compared to other sensors, but after this change it will be pushed like any other sensors.

I think the current implementation might not be too bad for you actually. The filter runs in two different modes: one when flying and another one when not flying (on the ground or being carried around). I'm not completely sure of the details but the "non flying" mode is not supposed to use the model and should be close to what you want I think. You can determine which mode that is used by looking at the quadIsFlying variable in the kalman_core.c file.

The kalman filter is implemented in kalman_core.c and the measurement model used in tdoa can be found in mm_tdoa.c. At the top of kalman_core.c you will also find a reference to the paper that the code was based on.
pimpam
Beginner
Posts: 19
Joined: Fri Aug 07, 2020 8:07 am

Re: How to log bare DWM1000 positioning data?

Post by pimpam »

Thank you for your suggestions. However, I still haven't totally understood that which parts I should modify. As I have a look on "kalman_core.c", also "mm_tdoa.c", there're just a bunch of functions to estimate the position from each sensor. However, I haven't seen any parts of the code or any other files outside, which is the main function that will call those functions at all. Could you please point out the correct way to go or give me more explanations about how I can deal with it?

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

Re: How to log bare DWM1000 positioning data?

Post by kristoffer »

Yes, there are a few files involved but the data flow is roughly something like this:

1. Sensor data is pushed into the estimator through the interface in https://github.com/bitcraze/crazyflie-f ... ator.h#L45, the estimatorEnqueueTDOA() function in the TDoA case. Search the code base for "estimatorEnqueue" to find where various sensors are scanned and pushed.
2. The active estimator (it is a plug in system) implements the interface and in your case (using the kalman estimator and TDoA) data will be pushed to estimatorKalmanEnqueueTDOA() (see https://github.com/bitcraze/crazyflie-f ... man.c#L620). The TDoA data is appended to a queue in the estimator to be processed later.
3. A task in the kalman estimator consumes data from the queue (https://github.com/bitcraze/crazyflie-f ... #L542-L552) and pushes it into the kalman core. The default flow is to call kalmanCoreUpdateWithTDOA()
4. The TDoA specific part of kalman core is implemented in mm_tdoa.c (https://github.com/bitcraze/crazyflie-f ... tdoa.c#L33) but at the end you have a call to kalmanCoreScalarUpdate() which updates the kalman filter.

The central parts of the kalman estimator implementation can be found in kalman_core.c (https://github.com/bitcraze/crazyflie-f ... man_core.c) but the process is driven by calls from estimator_kalman.c

IMU data (accelerometers and gyros) are handled in kalman_core.c, there is some averaging going on around lines 460 - 480 but the interesting part for you is probably in kalmanCorePredict(). You can see that gyro and acc data is handled differently depending on if quadIsFlying is true or false.

Note that IMU data is currently handled differently compared to other sensor data. There is a pull request to fix this (https://github.com/bitcraze/crazyflie-firmware/pull/735) that we plan to merge when the next release is out (in couple of days)
pimpam
Beginner
Posts: 19
Joined: Fri Aug 07, 2020 8:07 am

Re: How to log bare DWM1000 positioning data?

Post by pimpam »

Thank you so much for your explanation. I'll try my best to understand and make it works. I'll keep updating.
pimpam
Beginner
Posts: 19
Joined: Fri Aug 07, 2020 8:07 am

Re: How to log bare DWM1000 positioning data?

Post by pimpam »

Just for updating! I succeeded it by removing the part when !quadIsFlying in kalmanCorePredict(). Thank you so much Kristoffer for your nice explanation.
Post Reply