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