Absolute position of the Crazyflie in Kalman filter

All discussions related to the Loco Positioning system
Post Reply
cld
Beginner
Posts: 6
Joined: Thu Mar 05, 2020 1:02 pm

Absolute position of the Crazyflie in Kalman filter

Post by cld » Tue Jul 07, 2020 2:58 pm

Hello,

I would need some help with the "use" of the Kalman filter.

My goal:
We are trying to use the Crazyflies P2P feature in order to deploy several of them at the same time.
According to the P2P features described here it is possible to use the "inter-drone" network to send messages and therefore I assume that, in principle, it is possible to calculate the distance between the drones via rssi and/or time of flight of the signal. This is what we are after at the moment.
More specifically, the "use case" that I would like to develop now is the following:
We have two drones, maybe not even flying at the moment but just stationary on the ground and just one with the LPS tag.
On one hand, we might assume to know the position of one of them in a "global frame" if we have the LPS.
On the other hand, if the LPS is not there (then the LPS tag on the drone is not needed) we could assume that one drone defines the origin of our global system and the other one shall be localized within it.
As mentioned, to do this we want to use the signals used for the P2P feature.

What I understood:
If I understood it well the kalmanCoreUpdateWithDistance function in the Kalman_core.c module is the one that uses the distances measurements in the Kalman filter. From a first glance I assume that we could reuse this function for our purposes, maybe with minor changes.
At the moment this function seems to be mainly used when the LPS is used in TWR mode.
When the LPS is used in TWR mode, it uses the distance measurements which come from lpsTwrTag.c in src/drivers/src.

My question:
In kalmanCoreUpdateWithDistance the absolute position of the drone is used here:

Code: Select all

void kalmanCoreUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t *d)
{
  // a measurement of distance to point (x, y, z)
  float h[KC_STATE_DIM] = {0};
  arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};

  float dx = this->S[KC_STATE_X] - d->x;
  float dy = this->S[KC_STATE_Y] - d->y;
  float dz = this->S[KC_STATE_Z] - d->z;

  float measuredDistance = d->distance;

  float predictedDistance = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
  if (predictedDistance != 0.0f)
  {
    // The measurement is: z = sqrt(dx^2 + dy^2 + dz^2). The derivative dz/dX gives h.
    h[KC_STATE_X] = dx/predictedDistance;
    h[KC_STATE_Y] = dy/predictedDistance;
    h[KC_STATE_Z] = dz/predictedDistance;
  }
  else
  {
    // Avoid divide by zero
    h[KC_STATE_X] = 1.0f;
    h[KC_STATE_Y] = 0.0f;
    h[KC_STATE_Z] = 0.0f;
  }

  scalarUpdate(this, &H, measuredDistance-predictedDistance, d->stdDev);
}
However, it is not really clear to me how the global position of the drone gets there and how is that defined.
That is, since this function is used when the LPS in TWR mode, somehow the Crazyflie shall know its position within the LPS system, that is, with respect to the global frame defined in the LPS by manually setting the positions of the anchors. However, I do not manage to figure out how this information is passed to or used by the Crazyflie.

The way this info is passed to/used by the Crazyflie is relevant to me since I might probably use a similar approach to send the position (or the estimated position ) in the global frame of the first drone in order to have the second drone estimating its position with respect to the other.

Hoping that I was clear, I am looking forward to some support :)



kimberly
Bitcraze
Posts: 456
Joined: Fri Jul 06, 2018 11:13 am

Re: Absolute position of the Crazyflie in Kalman filter

Post by kimberly » Thu Jul 09, 2020 8:34 am

Hi! Let me try to help you further with this.
  • Your goal: Just a question about this, do you release that the p2p communication functionality is just about communicating through the NRF/Crazyradio? It is not p2p for the LPS deck. So just checking if you are aware of that. P2P on the LPS deck has not been implemented yet but you can check out some discussions on the issue list and the pull request from several people that are working on it now.
So about your question, I assume that you are talking about this part:

Code: Select all

  float dx = this->S[KC_STATE_X] - d->x;
  float dy = this->S[KC_STATE_Y] - d->y;
  float dz = this->S[KC_STATE_Z] - d->z;
and that you question is about, why we use the previous estimated absolute state in the kalman filter.

So for the kalman filter process is that it needs the difference between the measured distance (from CF to node) and the predicted distance (which it calculates based on the previous state). The last is calculated by a measurement model which uses previous state inputs in order to find out. Part of the distance_measurement_t is also the actual anchor positions (x,y,z), so if you take the difference between the anchor abs. pos. and the crazyflies estimated abs. position and then do this:

Code: Select all

  float predictedDistance = arm_sqrt(powf(dx, 2) + powf(dy, 2) + powf(dz, 2));
Then voila, you have the predicted distance that you can then substract from the actual measured distance so that you can tell the kalman filter how accurate it is based on the linearized measurement model that you send with it.

Why the kalmanfilter needs that, that requires knowledge on how exactly kalman filters work. You can check out this documentation, which forwards you to some other useful links that explain the basics of the extended kalman filter that the crazyflie uses. In the documentation we haven't added the expansion of the measurement model of the LPS deck, but its on our todo list.

Hope this answers your question?

cld
Beginner
Posts: 6
Joined: Thu Mar 05, 2020 1:02 pm

Re: Absolute position of the Crazyflie in Kalman filter

Post by cld » Thu Jul 09, 2020 1:21 pm

Hello!

Thanks a lot for your reply! :)

As it regards your first clarification:
Your goal: Just a question about this, do you release that the p2p communication functionality is just about communicating through the NRF/Crazyradio? It is not p2p for the LPS deck. So just checking if you are aware of that. P2P on the LPS deck has not been implemented yet but you can check out some discussions on the issue list and the pull request from several people that are working on it now.
Perfect!
This is what I understood too :)
My goal is to use the NRF based communication in a similar way to the LPS.
That is, I am trying to figure out if it is possible to get an estimate of the relative distances of the drones using the "NRF communication system" (not sure if this nomenclature is the correct one but I hope it is clear what I am referring to :) )
Since the RSSI is straightforward available from the P2P packet then it should be "easy" to use this in order to get a rough estimate of the relative distance based on RSSI.
However, I am trying to figure out if from the NRF is possible to get also some sort of timestamp for the broadcasted messages in order to use also a sort of "two-way-raning" approach similarly to the approach used with the LSP.

Do you have any input on this possibility?
I am checking also the info about the NRF51 SoC but it looks like that it might not be really "simple" to do it :roll:
See for example this forum discussion here

As it regards the Kalman filter:
Thanks for the answer!
It clarifies what was unclear to me :)

kimberly
Bitcraze
Posts: 456
Joined: Fri Jul 06, 2018 11:13 am

Re: Absolute position of the Crazyflie in Kalman filter

Post by kimberly » Fri Jul 10, 2020 8:53 am

Ah good that you already understood it. NRF based localization will be challenging but definitely a very interesting feature. I worked on some RSSI related stuff during my PhD (where the crazyflies had to go back to the crazyradio beacon and avoid eachother with P2P), but that was just as a measure of progression or a measure of 'closeness', so I didn't even touch the whole localization estimation part.

However I'm not saying that it is not impossible! I was a coauthor of a paper where my previous colleague try to do the same but with bluetooth (see this paper). In general I would say that the noise of BLE was much worse than the crazyradio in the RSSI, so... you might have something there.

About the timestamps, this might be though since you need to sync the NRFs with each-other somehow, and I wouldn't know exactly how to do that. You could sync up at the beginning with a package of one crazyflie with their timestamp, but then you will need to take into account the delay as well. And ofcourse in time, the internal clocks will divert again slightly. This needs to be investigated how to do this, but to be honest, I think the RSSI might be just a bit too noisy to make that much of a difference (but I'm not super sure about this).

cld
Beginner
Posts: 6
Joined: Thu Mar 05, 2020 1:02 pm

Re: Absolute position of the Crazyflie in Kalman filter

Post by cld » Tue Jul 21, 2020 2:38 pm

Thanks a lot for your reply Kimberly! :)
but that was just as a measure of progression or a measure of 'closeness', so I didn't even touch the whole localization estimation part.
I looked a bit more into this and RSSI looks indeed a nice tool for proximity but not really much for distance.
Using it for measuring the distance between crazyflies gives errors of the order of a few meters.
Placing two crazyflies at 1m I got values from 0.5m to 2m.
Moving them to 2m distances the estimated distance easily gives values from 3m to 7m.
Maybe it is possible to improve it a bit but I do not expect anything really reliable for relative positioning :/
In conclusion, I agree with you here
I think the RSSI might be just a bit too noisy to make that much of a difference (but I'm not super sure about this).
About the timestamps:
About the timestamps, this might be though since you need to sync the NRFs with each-other somehow, and I wouldn't know exactly how to do that. You could sync up at the beginning with a package of one crazyflie with their timestamp, but then you will need to take into account the delay as well. And ofcourse in time, the internal clocks will divert again slightly. This needs to be investigated how to do this,
I looked a bit also into this.
In principle it looks like that the nRF51 shall offer the possibility to implement some sort of clock synchronization feature.
I found this topic on the Nordic Semiconductor forum here.
I also found this examples on how to implement the clock synchronization feature for the nRF51 - here.
However, I am struggling a bit to understand how and if it is even possible to implement this feature in the nrf firmware for the Crazyflie.
I am having some troubles to understand how actually enter the code in the nrf firmware with the pieces of code developed in the example.
Any hint?

The example that I posted seems to be relevant for the nrf51 DK but I assume that in principle it should be possible to include it also in the Crazyflie.
Not sure if the requirements are supported though since the time sync example seems to require at least the SDK 14.2.0 while the nrf in the Crazyflie seems to use the SDK 6.10 (if I am correct).

kimberly
Bitcraze
Posts: 456
Joined: Fri Jul 06, 2018 11:13 am

Re: Absolute position of the Crazyflie in Kalman filter

Post by kimberly » Wed Jul 22, 2020 8:00 am

That example looks like a nice feature to include in the NRF for sure. I'm not sure if I have time to do this within these weeks but I would definitely advise to make an issue for this on the NRF repo: https://github.com/bitcraze/crazyflie2-nrf-firmware to get the discussion going!

The NRF code is simpler than the crazyflie firmware since it does not use Freertos for the scheduling (so everything is just running in one big loop), however we will probably need to add something to the p2p in order to accommodate this. Also the example should not contain any blocking functions (while loops that goes on for every or blocking sleep functions).

Anyway, lets discuss further there! I will see if I can nudge some people to join the conversation but it is the holiday period though ;)

cld
Beginner
Posts: 6
Joined: Thu Mar 05, 2020 1:02 pm

Re: Absolute position of the Crazyflie in Kalman filter

Post by cld » Wed Jul 22, 2020 11:46 am

Thanks Kimberly! :)

I just opened the issue on Github - here -

It is vacation time also here but I will try to follow up the discussion eventually ;)

kimberly
Bitcraze
Posts: 456
Joined: Fri Jul 06, 2018 11:13 am

Re: Absolute position of the Crazyflie in Kalman filter

Post by kimberly » Wed Jul 22, 2020 2:00 pm

Thanks! Probably I will assign arnaud to this as well, but he is on parental leave until august, but it is interesting stuff for sure!

Post Reply