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);
}
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
