Bitcraze Forums

Posted: **Mon Feb 17, 2020 3:32 am**

Hello,

I am currently implementing an Extended Kalman Filter for some ground robots using the LPS system. I was looking at your code-base and saw that instead of computing the location from the TDoA distance value, you're computing the error between the measured and the predicted distance values and then using that single scalar as the innovation in the KF algorithm. To do this, however, you are implementing the equations within the observation matrix to map this error back into the state space of the Crazyflie. I was wondering what equations you used to compute the Jacobian? The relevant code section is found here, and I will paste the code that I am looking at specifically:

Code: Select all

void kalmanCoreUpdateWithTDOA(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa){

...

  if ((d0 != 0.0f) && (d1 != 0.0f)) {
      h[KC_STATE_X] = (dx1 / d1 - dx0 / d0);
      h[KC_STATE_Y] = (dy1 / d1 - dy0 / d0);
      h[KC_STATE_Z] = (dz1 / d1 - dz0 / d0);

      vector_t jacobian = {
        .x = h[KC_STATE_X],
        .y = h[KC_STATE_Y],
        .z = h[KC_STATE_Z],
      };

      point_t estimatedPosition = {
        .x = this->S[KC_STATE_X],
        .y = this->S[KC_STATE_Y],
        .z = this->S[KC_STATE_Z],
      };

      bool sampleIsGood = outlierFilterValidateTdoaSteps(tdoa, error, &jacobian, &estimatedPosition);
      if (sampleIsGood) {
        scalarUpdate(this, &H, error, tdoa->stdDev);
      }
  
     ...
     
}

Thank you in advance.

Posted: **Mon Feb 17, 2020 3:14 pm**

Hi! and thank you for your question.

So technically, TDOA is the measured difference between vector lengths between one anchor and the crazyflie and the another anchor to the crazyflie. The predicted TDOA is calculated by using the estimated position and the current anchors location, which is done here, as you already found out.

So how to calculate the jacobian, please see this handwritten derivation here:

so that I use the dX, dY and dZ instead of the estimated X,Y,Z of the crazyflie (since dX = X-Xn), is just a simplification of the derivation. If you would check wolfram alpha with : 'differential of ((x-d)^2 +(y-e)^2+(z-f)^2)^(1/2)-((x-a)^2 +(y-b)^2+(z-c)^2)^(1/2)', you would see that there is actually no difference in the way how the jacobian is calculated

Hope this is clear?

Posted: **Mon Feb 17, 2020 6:07 pm**

Yes, thank you!

Bitcraze Forums

[SOLVED] Extended Kalman Filter Update with TDoA2 observation matrix

[SOLVED] Extended Kalman Filter Update with TDoA2 observation matrix

Re: Extended Kalman Filter Update with TDoA2 observation matrix

Re: Extended Kalman Filter Update with TDoA2 observation matrix