[SOLVED] Extended Kalman Filter Update with TDoA2 observation matrix

All discussions related to the Loco Positioning system
Post Reply
snyderthorst
Beginner
Posts: 23
Joined: Thu Jun 28, 2018 12:00 am

[SOLVED] Extended Kalman Filter Update with TDoA2 observation matrix

Post by snyderthorst »

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.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Extended Kalman Filter Update with TDoA2 observation matrix

Post by kimberly »

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:
Image from iOS (1).jpg
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?
snyderthorst
Beginner
Posts: 23
Joined: Thu Jun 28, 2018 12:00 am

Re: Extended Kalman Filter Update with TDoA2 observation matrix

Post by snyderthorst »

Yes, thank you!
Post Reply