Skip to content

Commit

Permalink
Removed old tdoa counter, not needed anymore
Browse files Browse the repository at this point in the history
  • Loading branch information
@krichardsson
krichardsson committed Feb 28, 2023
1 parent b3de613 commit b01aa75
Show file tree
Hide file tree
Showing 3 changed files with 94 additions and 114 deletions.
80 changes: 37 additions & 43 deletions src/modules/src/estimator_ukf.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,6 @@ static float baroOut;
static float PxOut;
static float PvxOut;
static float PattxOut;
static uint32_t tdoaCount;

static bool useNavigationFilter = true;
static bool resetNavigation = true;
Expand Down Expand Up @@ -352,7 +351,6 @@ static void errorUkfTask(void *parameters)
accNed[1] = 0.0f;
accNed[2] = 0.0f;

tdoaCount = 0;
nanCounterFilter = 0;

navigationInit();
Expand Down Expand Up @@ -826,59 +824,55 @@ static bool updateQueuedMeasurements(const uint32_t tick, Axis3f *gyroAverage)
switch (m.type)
{
case MeasurementTypeTDOA:
if (tdoaCount >= 100)
//_________________________________________________________________________________
// UKF update - TDOA
//_________________________________________________________________________________
// compute mean tdoa observation
observation = 0.0f;
for (jj = 0; jj < (DIM_FILTER + 2); jj++)
{
//_________________________________________________________________________________
// UKF update - TDOA
//_________________________________________________________________________________
// compute mean tdoa observation
observation = 0.0f;
for (jj = 0; jj < (DIM_FILTER + 2); jj++)
for (ii = 0; ii < DIM_FILTER; ii++)
{
for (ii = 0; ii < DIM_FILTER; ii++)
{
tmpSigmaVec[ii] = sigmaPoints[ii][jj];
}
computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
observation = observation + weights[jj] * outTmp;
tmpSigmaVec[ii] = sigmaPoints[ii][jj];
}
computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
observation = observation + weights[jj] * outTmp;
}

// initialize measurement and cross covariance
Pyy = 0.0f;
for (jj = 0; jj < DIM_FILTER; jj++)
// initialize measurement and cross covariance
Pyy = 0.0f;
for (jj = 0; jj < DIM_FILTER; jj++)
{
Pxy[jj] = 0.0f;
}

// loop over all sigma points
for (jj = 0; jj < (DIM_FILTER + 2); jj++)
{
for (ii = 0; ii < DIM_FILTER; ii++)
{
Pxy[jj] = 0.0f;
tmpSigmaVec[ii] = sigmaPoints[ii][jj];
}
computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
Pyy = Pyy + weights[jj] * (outTmp - observation) * (outTmp - observation);

// loop over all sigma points
for (jj = 0; jj < (DIM_FILTER + 2); jj++)
for (kk = 0; kk < DIM_FILTER; kk++)
{
for (ii = 0; ii < DIM_FILTER; ii++)
{
tmpSigmaVec[ii] = sigmaPoints[ii][jj];
}
computeOutputTdoa(&outTmp, &tmpSigmaVec[0], &m.data.tdoa);
Pyy = Pyy + weights[jj] * (outTmp - observation) * (outTmp - observation);

for (kk = 0; kk < DIM_FILTER; kk++)
{
Pxy[kk] = Pxy[kk] + weights[jj] * (tmpSigmaVec[kk] - xEst[kk]) * (outTmp - observation);
}
Pxy[kk] = Pxy[kk] + weights[jj] * (tmpSigmaVec[kk] - xEst[kk]) * (outTmp - observation);
}
}

// Add TDOA Noise R
Pyy = Pyy + m.data.tdoa.stdDev * m.data.tdoa.stdDev;
innovation = m.data.tdoa.distanceDiff - observation;
// Add TDOA Noise R
Pyy = Pyy + m.data.tdoa.stdDev * m.data.tdoa.stdDev;
innovation = m.data.tdoa.distanceDiff - observation;

innoCheck = innovation * innovation / Pyy;
if (outlierFilterTdoaValidateIntegrator(&outlierFilterTdoaState, &m.data.tdoa, innovation, nowMs))
{
// if(innoCheck<qualGateTdoa){ // TdoA outlier rejection
ukfUpdate(&Pxy[0], &Pyy, innovation);
// }
}
innoCheck = innovation * innovation / Pyy;
if (outlierFilterTdoaValidateIntegrator(&outlierFilterTdoaState, &m.data.tdoa, innovation, nowMs))
{
// if(innoCheck<qualGateTdoa){ // TdoA outlier rejection
ukfUpdate(&Pxy[0], &Pyy, innovation);
// }
}
tdoaCount++;
break;

case MeasurementTypeTOF:
Expand Down
122 changes: 57 additions & 65 deletions src/modules/src/kalman_core/mm_tdoa.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,73 +30,65 @@
#include "outlierFilterTdoaSteps.h"
#endif

// TODO krri What is this used for? Do we still need it?
TESTABLE_STATIC uint32_t tdoaCount = 0;

void kalmanCoreUpdateWithTdoa(kalmanCoreData_t* this, tdoaMeasurement_t *tdoa, const uint32_t nowMs, OutlierFilterTdoaState_t* outlierFilterState)
{
if (tdoaCount >= 100)
{
/**
* Measurement equation:
* dR = dT + d1 - d0
*/

float measurement = tdoa->distanceDiff;

// predict based on current state
float x = this->S[KC_STATE_X];
float y = this->S[KC_STATE_Y];
float z = this->S[KC_STATE_Z];

float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;

float dx1 = x - x1;
float dy1 = y - y1;
float dz1 = z - z1;

float dy0 = y - y0;
float dx0 = x - x0;
float dz0 = z - z0;

float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));

float predicted = d1 - d0;
float error = measurement - predicted;

float h[KC_STATE_DIM] = {0};
arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};

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

#if CONFIG_ESTIMATOR_KALMAN_TDOA_OUTLIERFILTER_FALLBACK
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 = outlierFilterTdoaValidateSteps(tdoa, error, &jacobian, &estimatedPosition);
#else
bool sampleIsGood = outlierFilterTdoaValidateIntegrator(outlierFilterState, tdoa, error, nowMs);
#endif

if (sampleIsGood) {
kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
}
/**
* Measurement equation:
* dR = dT + d1 - d0
*/

float measurement = tdoa->distanceDiff;

// predict based on current state
float x = this->S[KC_STATE_X];
float y = this->S[KC_STATE_Y];
float z = this->S[KC_STATE_Z];

float x1 = tdoa->anchorPositions[1].x, y1 = tdoa->anchorPositions[1].y, z1 = tdoa->anchorPositions[1].z;
float x0 = tdoa->anchorPositions[0].x, y0 = tdoa->anchorPositions[0].y, z0 = tdoa->anchorPositions[0].z;

float dx1 = x - x1;
float dy1 = y - y1;
float dz1 = z - z1;

float dy0 = y - y0;
float dx0 = x - x0;
float dz0 = z - z0;

float d1 = sqrtf(powf(dx1, 2) + powf(dy1, 2) + powf(dz1, 2));
float d0 = sqrtf(powf(dx0, 2) + powf(dy0, 2) + powf(dz0, 2));

float predicted = d1 - d0;
float error = measurement - predicted;

float h[KC_STATE_DIM] = {0};
arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};

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

#if CONFIG_ESTIMATOR_KALMAN_TDOA_OUTLIERFILTER_FALLBACK
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 = outlierFilterTdoaValidateSteps(tdoa, error, &jacobian, &estimatedPosition);
#else
bool sampleIsGood = outlierFilterTdoaValidateIntegrator(outlierFilterState, tdoa, error, nowMs);
#endif

if (sampleIsGood) {
kalmanCoreScalarUpdate(this, &H, error, tdoa->stdDev);
}
}

tdoaCount++;
}
6 changes: 0 additions & 6 deletions test/modules/src/kalman_core/test_mm_tdoa.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,10 @@ static kalmanCoreData_t this;
static float expectedHm[KC_STATE_DIM];
static OutlierFilterTdoaState_t outlierFilterTdoaState;

// Instrumented in code under test
extern uint32_t tdoaCount;

void setUp(void) {
memset(&this, 0, sizeof(this));
memset(&expectedHm, 0, sizeof(expectedHm));

// Make sure we pass the tdoaCount counter
tdoaCount = 100;

initKalmanCoreScalarUpdateExpectationsSingleCall();

outlierFilterTdoaReset_Ignore();
Expand Down

0 comments on commit b01aa75

Please sign in to comment.