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: 2275
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: 280
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.

Post Reply