Skip to content

Commit

Permalink
#163 Changed API for tdoa kalman estimator.
Browse files Browse the repository at this point in the history 
Now uses difference in distance instead of absolute times.
  • Loading branch information
@krichardsson
krichardsson committed Nov 23, 2016
1 parent 23197b2 commit 6ecbff0
Show file tree
Hide file tree
Showing 4 changed files with 112 additions and 112 deletions.
67 changes: 21 additions & 46 deletions src/deck/drivers/src/lpsTdoaTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,29 +40,20 @@
static lpsAlgoOptions_t* options;

float uwbTdoaDistDiff[LOCODECK_NR_OF_ANCHORS];
static toaMeasurement_t lastTOA;

static rangePacket_t rxPacketBuffer[LOCODECK_NR_OF_ANCHORS];
static dwTime_t arrivals[LOCODECK_NR_OF_ANCHORS];

static double frameTime_in_cl_M = 0.0;
static double clockCorrection_T_To_M = 1.0;

static int64_t tagClockWrapOffset = 0;
static int64_t tagClockLatestTime = 0;

static int64_t masterClockWrapOffset = 0;
static int64_t masterClockLatestTime = 0;

typedef struct {
int64_t offset;
int64_t latestTime;
} clockWrap_t;

static clockWrap_t clockWrapTag, clockWrapMaster;

#define MASTER 0
#define MEASUREMENT_NOISE_STD 0.5f
#define MEASUREMENT_NOISE_STD 10.0f

// The maximum diff in distances that we consider to be valid
// Used to sanity check results and remove results that are wrong due to packet loss
Expand All @@ -79,29 +70,25 @@ static uint64_t truncateToTimeStamp(uint64_t fullTimeStamp) {
return fullTimeStamp & 0x00FFFFFFFFFFul;
}

static int64_t eliminateClockWrap(clockWrap_t* data, int64_t time) {
if ((time < data->latestTime)) {
data->offset += 0x10000000000;
}

data->latestTime = time;

return time + data->offset;
}

static void enqueueTDOA(uint8_t anchor, int64_t rxAn_by_T_in_cl_T, int64_t txAn_in_cl_M) {
tdoaMeasurement_t tdoa = {.stdDev = MEASUREMENT_NOISE_STD};

memcpy(&(tdoa.measurement[0]), &lastTOA, sizeof(toaMeasurement_t));

tdoa.measurement[1].senderId = anchor;
tdoa.measurement[1].rx = eliminateClockWrap(&clockWrapTag, rxAn_by_T_in_cl_T);
tdoa.measurement[1].tx = eliminateClockWrap(&clockWrapMaster, txAn_in_cl_M);
tdoa.measurement[1].x = options->anchorPosition[anchor].x;
tdoa.measurement[1].y = options->anchorPosition[anchor].y;
tdoa.measurement[1].z = options->anchorPosition[anchor].z;
static void enqueueTDOA(uint8_t anchor, double distanceDiff, double timeBetweenMeasurements) {
tdoaMeasurement_t tdoa = {
.stdDev = MEASUREMENT_NOISE_STD,
.distanceDiff = distanceDiff,
.timeBetweenMeasurements = timeBetweenMeasurements,

.anchorPosition[0] = {
.x = options->anchorPosition[0].x,
.y = options->anchorPosition[0].y,
.z = options->anchorPosition[0].z
},

.anchorPosition[1] = {
.x = options->anchorPosition[anchor].x,
.y = options->anchorPosition[anchor].y,
.z = options->anchorPosition[anchor].z
}
};

memcpy(&lastTOA, &tdoa.measurement[1], sizeof(toaMeasurement_t));
#ifdef ESTIMATOR_TYPE_kalman
stateEstimatorEnqueueTDOA(&tdoa);
#endif
Expand Down Expand Up @@ -137,8 +124,6 @@ static void rxcallback(dwDevice_t *dev) {
if (frameTime_in_T != 0.0) {
clockCorrection_T_To_M = frameTime_in_cl_M / frameTime_in_T;
}

enqueueTDOA(MASTER, rxAn_by_T_in_cl_T, txM_in_cl_M);
} else {
int64_t previous_txAn_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
int64_t rxAn_by_M_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[anchor]);
Expand All @@ -159,14 +144,14 @@ static void rxcallback(dwDevice_t *dev) {
int64_t tof_M_to_An_in_cl_M = (((truncateToTimeStamp(rxM_by_An_in_cl_An - previous_txAn_in_cl_An) * clockCorrection_An_To_M) - truncateToTimeStamp(txM_in_cl_M - rxAn_by_M_in_cl_M))) / 2.0;
int64_t delta_txM_to_txAn_in_cl_M = (tof_M_to_An_in_cl_M + truncateToTimeStamp(txAn_in_cl_An - rxM_by_An_in_cl_An) * clockCorrection_An_To_M);
int64_t timeDiffOfArrival_in_cl_M = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T) * clockCorrection_T_To_M - delta_txM_to_txAn_in_cl_M;
int64_t txAn_in_cl_M = txM_in_cl_M + delta_txM_to_txAn_in_cl_M;

float tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_M / LOCODECK_TS_FREQ;
float timeBetweenMeasurements = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T) / LOCODECK_TS_FREQ;

// Sanity check distances in case of missed packages
if (tdoaDistDiff > -MAX_DISTANCE_DIFF && tdoaDistDiff < MAX_DISTANCE_DIFF) {
uwbTdoaDistDiff[anchor] = tdoaDistDiff;
enqueueTDOA(anchor, rxAn_by_T_in_cl_T, txAn_in_cl_M);
enqueueTDOA(anchor, tdoaDistDiff, timeBetweenMeasurements);
}
}

Expand Down Expand Up @@ -207,21 +192,11 @@ static void Initialize(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions) {

// Reset module state. Needed by unit tests
memset(uwbTdoaDistDiff, 0, sizeof(uwbTdoaDistDiff));
memset(&lastTOA, 0, sizeof(lastTOA));
memset(rxPacketBuffer, 0, sizeof(rxPacketBuffer));
memset(arrivals, 0, sizeof(arrivals));

frameTime_in_cl_M = 0.0;
clockCorrection_T_To_M = 1.0;

tagClockWrapOffset = 0;
tagClockLatestTime = 0;

masterClockWrapOffset = 0;
masterClockLatestTime = 0;

memset(&clockWrapTag, 0, sizeof(clockWrapTag));
memset(&clockWrapMaster, 0, sizeof(clockWrapMaster));
}
#pragma GCC diagnostic pop

Expand Down
10 changes: 3 additions & 7 deletions src/modules/interface/stabilizer_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,14 +76,10 @@ typedef struct quaternion_s {
};
} quaternion_t;

typedef struct toaMeasurement_s {
int8_t senderId;
float x, y, z;
int64_t rx, tx;
} toaMeasurement_t;

typedef struct tdoaMeasurement_s {
toaMeasurement_t measurement[2];
point_t anchorPosition[2];
float distanceDiff;
float timeBetweenMeasurements;
float stdDev;
} tdoaMeasurement_t;

Expand Down
29 changes: 7 additions & 22 deletions src/modules/src/estimator_kalman.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -875,43 +875,28 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa)
{
/**
* Measurement equation:
* dR = dT + dT*skew + d1 - d0
* dR = dT + d1 - d0
*/

// We cannot accurately position, until we have an accurate knowledge of skew.
// We assume that skew is uncorrelated with other quad states, we can therefore track it
// using a separate, 1D Kalman filter. Here, we predict the skew forward
float dt = (float)(tdoa->measurement[1].rx - tdoa->measurement[0].rx)*SECONDS_PER_TDOATICK;
if (dt > 0)
{
// We assume skew stays constant, but increase the variance
varSkew += powf(procNoiseSkew * dt, 2); // compute variance from standard deviation
stateEstimatorAddProcessNoise(dt); // add process noise occurring between packet receptions
}
float dt = tdoa->timeBetweenMeasurements;
stateEstimatorAddProcessNoise(dt); // add process noise occurring between packet receptions

// calculate the TDOA measurement
int64_t dR = tdoa->measurement[1].rx - tdoa->measurement[0].rx;
int64_t dT = tdoa->measurement[1].tx - tdoa->measurement[0].tx;
float measurement = METERS_PER_TDOATICK*(dR - dT); // possible to lose precision here in floats
float measurement = tdoa->distanceDiff;

// predict based on current state
float x = S[STATE_X];
float y = S[STATE_Y];
float z = S[STATE_Z];

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

float d1 = sqrtf(powf(x - x1, 2) + powf(y - y1, 2) + powf(z - z1, 2));
float d0 = sqrtf(powf(x - x0, 2) + powf(y - y0, 2) + powf(z - z0, 2));

float predicted = METERS_PER_TDOATICK * dT * stateSkew + (d1 - d0);
float predicted = d1 - d0;
float error = measurement - predicted;
float varUwb = tdoa->stdDev*tdoa->stdDev*dt*dt;
float Hs = METERS_PER_TDOATICK * dT; // derivative of prediction equation with respect to stateSkew
float Ks = varSkew * Hs / (Hs * varSkew * Hs + varUwb);
stateSkew = (stateSkew + Ks * error);
varSkew = ((1 - Ks * Hs) * varSkew * (1 - Ks * Hs) + Ks * varUwb * Ks);

if (tdoaCount >= 100)
{
Expand Down
118 changes: 81 additions & 37 deletions test/deck/drivers/src/TestLpsTdoaTag.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ static void mockMessageFromAnchor(uint8_t anchorIndex, uint64_t rxTime, uint64_t
static void mockRadioSetToReceiveMode();

static void ignoreKalmanEstimatorValidation();
static void mockKalmanEstimator(uint8_t anchor, uint64_t arrivalTime, uint64_t txTime);
static void mockKalmanEstimator(uint8_t anchor, double distanceDiff, double timeBetweenMeasurements);
static void mockKalmanEstimator_resetMock();

static uint64_t drift(float factor, uint64_t time);
Expand Down Expand Up @@ -142,10 +142,8 @@ void testKalmanEstimatorWithNoClockDrift() {
mockMessageFromAnchor(1, iTxTime3 + timeA1ToTag + tO, iTxTime2 + timeA0ToA1 + a1O, iTxTime3 + a1O , NS, NS, NS, NS, NS, NS);

// A Arrival Time in tag clock Tx time in A0 clock
mockKalmanEstimator(0, iTxTime0 + timeA0ToTag + tO, iTxTime0 + a0O);
// mockKalmanEstimator(1, iTxTime1 + timeA1ToTag + tO, iTxTime1 + a0O); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(0, iTxTime2 + timeA0ToTag + tO, iTxTime2 + a0O);
mockKalmanEstimator(1, iTxTime3 + timeA1ToTag + tO, iTxTime3 + a0O);
// mockKalmanEstimator(1, 2.5 - 2.0, , ((iTxTime1 + timeA1ToTag + tO) - (iTxTime0 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime3 + timeA1ToTag + tO) - (iTxTime2 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ);

// Test
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
Expand Down Expand Up @@ -234,10 +232,54 @@ void testKalmanEstimatorWithNoClockDriftWithTagClockWrapping() {
mockMessageFromAnchor(1, iTxTime3 + timeA1ToTag + tO, iTxTime2 + timeA0ToA1 + a1O, iTxTime3 + a1O , NS, NS, NS, NS, NS, NS);

// A Arrival Time in tag clock Tx time in A0 clock
mockKalmanEstimator(0, iTxTime0 + timeA0ToTag + tO, iTxTime0 + a0O);
// mockKalmanEstimator(1, iTxTime1 + timeA1ToTag + tO, iTxTime1 + a0O); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(0, iTxTime2 + timeA0ToTag + tO, iTxTime2 + a0O);
mockKalmanEstimator(1, iTxTime3 + timeA1ToTag + tO, iTxTime3 + a0O);
// mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime1 + timeA1ToTag + tO) - (iTxTime0 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime3 + timeA1ToTag + tO) - (iTxTime2 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ);

// Test
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);

// Assert
// Nothing here, verification in mocks
}

void testKalmanEstimatorWithNoClockDriftWithTagClockWrapping2() {
#ifndef ESTIMATOR_TYPE_kalman
TEST_IGNORE();
#endif
// Fixture
// Two anchors (A0 and A1), separated by 1.0m
// Distance from A0 to tag is 2.0m
// Distance from A1 to tag is 2.5m

// Ideal times in universal clock
uint64_t timeA0ToA1 = 1.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT;
uint64_t timeA0ToTag = 2.0 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT;
uint64_t timeA1ToTag = 2.5 * LOCODECK_TS_FREQ / SPEED_OF_LIGHT;

uint64_t iTxTime0 = 1.0 * LOCODECK_TS_FREQ;
uint64_t iTxTime1 = 1.002 * LOCODECK_TS_FREQ;
uint64_t iTxTime2 = 1.004 * LOCODECK_TS_FREQ;
uint64_t iTxTime3 = 1.006 * LOCODECK_TS_FREQ;

// Clock start offset including any antenna delay.
// Intentionally less than 40 bit clock wrap around, approx 17s
uint64_t a0O = 3 * LOCODECK_TS_FREQ;
uint64_t a1O = 2 * LOCODECK_TS_FREQ;
// Local clock start offset set to wrap after second message from A0
uint64_t tO = TIMER_MAX_VALUE - (iTxTime2 + timeA0ToTag) - 1;

// A Arrival Time A0 A1 A2 A3 A4 A5 A6 A7
mockMessageFromAnchor(0, iTxTime0 + timeA0ToTag + tO, iTxTime0 + a0O , NS , NS, NS, NS, NS, NS, NS);
mockMessageFromAnchor(1, iTxTime1 + timeA1ToTag + tO, iTxTime0 + timeA0ToA1 + a1O, iTxTime1 + a1O , NS, NS, NS, NS, NS, NS);
mockMessageFromAnchor(0, iTxTime2 + timeA0ToTag + tO, iTxTime2 + a0O , iTxTime1 + timeA0ToA1 + a0O, NS, NS, NS, NS, NS, NS);
mockMessageFromAnchor(1, iTxTime3 + timeA1ToTag + tO, iTxTime2 + timeA0ToA1 + a1O, iTxTime3 + a1O , NS, NS, NS, NS, NS, NS);

// A Arrival Time in tag clock Tx time in A0 clock
// mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime1 + timeA1ToTag + tO) - (iTxTime0 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime3 + timeA1ToTag + tO) - (iTxTime2 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ);

// Test
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
Expand Down Expand Up @@ -282,10 +324,8 @@ void testKalmanEstimatorWithNoClockDriftWithA0ClockWrapping() {
mockMessageFromAnchor(1, iTxTime3 + timeA1ToTag + tO, iTxTime2 + timeA0ToA1 + a1O, iTxTime3 + a1O , NS, NS, NS, NS, NS, NS);

// A Arrival Time in tag clock Tx time in A0 clock
mockKalmanEstimator(0, iTxTime0 + timeA0ToTag + tO, iTxTime0 + a0O);
// mockKalmanEstimator(1, iTxTime1 + timeA1ToTag + tO, iTxTime1 + a0O); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(0, iTxTime2 + timeA0ToTag + tO, iTxTime2 + a0O);
mockKalmanEstimator(1, iTxTime3 + timeA1ToTag + tO, iTxTime3 + a0O);
// mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime1 + timeA1ToTag + tO) - (iTxTime0 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime3 + timeA1ToTag + tO) - (iTxTime2 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ);

// Test
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
Expand Down Expand Up @@ -330,10 +370,8 @@ void testKalmanEstimatorWithNoClockDriftWithA1ClockWrapping() {
mockMessageFromAnchor(1, iTxTime3 + timeA1ToTag + tO, iTxTime2 + timeA0ToA1 + a1O, iTxTime3 + a1O , NS, NS, NS, NS, NS, NS);

// A Arrival Time in tag clock Tx time in A0 clock
mockKalmanEstimator(0, iTxTime0 + timeA0ToTag + tO, iTxTime0 + a0O);
// mockKalmanEstimator(1, iTxTime1 + timeA1ToTag + tO, iTxTime1 + a0O); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(0, iTxTime2 + timeA0ToTag + tO, iTxTime2 + a0O);
mockKalmanEstimator(1, iTxTime3 + timeA1ToTag + tO, iTxTime3 + a0O);
// mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime1 + timeA1ToTag + tO) - (iTxTime0 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ); Not called as the packet is interpreted as packet loss and filtered out
mockKalmanEstimator(1, 2.5 - 2.0, ((iTxTime3 + timeA1ToTag + tO) - (iTxTime2 + timeA0ToTag + tO)) / LOCODECK_TS_FREQ);

// Test
uwbTdoaTagAlgorithm.onEvent(&dev, eventPacketReceived);
Expand Down Expand Up @@ -864,43 +902,49 @@ static void ignoreKalmanEstimatorValidation() {
#ifdef ESTIMATOR_TYPE_kalman
#define STATE_ESTIMATOR_MAX_NR_OF_CALLS 10

static toaMeasurement_t stateEstimatorExpectations[STATE_ESTIMATOR_MAX_NR_OF_CALLS];
static tdoaMeasurement_t stateEstimatorExpectations[STATE_ESTIMATOR_MAX_NR_OF_CALLS];
static int stateEstimatorIndex = 0;

static void validateToa(const toaMeasurement_t* expected, const toaMeasurement_t* actual, int cmock_num_calls, int measurement) {
static bool stateEstimatorEnqueueTDOAMockCallback(tdoaMeasurement_t* actual, int cmock_num_calls) {
char message[100];
sprintf(message, "Failed in call %i to stateEstimatorEnqueueTDOA() for message %i", cmock_num_calls, measurement);
sprintf(message, "Failed in call %i to stateEstimatorEnqueueTDOA()", cmock_num_calls);

TEST_ASSERT_EQUAL_UINT64_MESSAGE(expected->senderId, actual->senderId, message);
TEST_ASSERT_EQUAL_UINT64_MESSAGE(expected->rx, actual->rx, message);
TEST_ASSERT_EQUAL_UINT64_MESSAGE(expected->tx, actual->tx, message);
tdoaMeasurement_t* expected = &stateEstimatorExpectations[cmock_num_calls];
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.01, expected->distanceDiff, actual->distanceDiff, message);

TEST_ASSERT_FLOAT_WITHIN(0.0, options.anchorPosition[expected->senderId].x, actual->x);
TEST_ASSERT_FLOAT_WITHIN(0.0, options.anchorPosition[expected->senderId].y, actual->y);
TEST_ASSERT_FLOAT_WITHIN(0.0, options.anchorPosition[expected->senderId].z, actual->z);
}
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->timeBetweenMeasurements, actual->timeBetweenMeasurements, message);

static bool stateEstimatorEnqueueTDOAMockCallback(tdoaMeasurement_t* uwb, int cmock_num_calls) {
if (cmock_num_calls > 0) {
validateToa(&stateEstimatorExpectations[cmock_num_calls - 1], &uwb->measurement[0], cmock_num_calls, 0);
}
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->anchorPosition[0].x, actual->anchorPosition[0].x, message);
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->anchorPosition[0].y, actual->anchorPosition[0].y, message);
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->anchorPosition[0].z, actual->anchorPosition[0].z, message);

validateToa(&stateEstimatorExpectations[cmock_num_calls], &uwb->measurement[1], cmock_num_calls, 1);
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->anchorPosition[1].x, actual->anchorPosition[1].x, message);
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->anchorPosition[1].y, actual->anchorPosition[1].y, message);
TEST_ASSERT_FLOAT_WITHIN_MESSAGE(0.0, expected->anchorPosition[1].z, actual->anchorPosition[1].z, message);

return true;
}
#endif

static void mockKalmanEstimator(uint8_t anchor, uint64_t arrivalTime, uint64_t txTime) {
static void mockKalmanEstimator(uint8_t anchor, double distanceDiff, double timeBetweenMeasurements) {
#ifdef ESTIMATOR_TYPE_kalman
TEST_ASSERT_TRUE(stateEstimatorIndex < STATE_ESTIMATOR_MAX_NR_OF_CALLS);

stateEstimatorEnqueueTDOA_StubWithCallback(stateEstimatorEnqueueTDOAMockCallback);

toaMeasurement_t* toa = &stateEstimatorExpectations[stateEstimatorIndex];
toa->senderId = anchor;
toa->rx = arrivalTime;
toa->tx = txTime;
tdoaMeasurement_t* measurement = &stateEstimatorExpectations[stateEstimatorIndex];

measurement->distanceDiff = distanceDiff;
measurement->timeBetweenMeasurements = timeBetweenMeasurements;

// Always comparing to anchor 0 for now
measurement->anchorPosition[0].x = options.anchorPosition[0].x;
measurement->anchorPosition[0].y = options.anchorPosition[0].y;
measurement->anchorPosition[0].z = options.anchorPosition[0].z;

measurement->anchorPosition[1].x = options.anchorPosition[anchor].x;
measurement->anchorPosition[1].y = options.anchorPosition[anchor].y;
measurement->anchorPosition[1].z = options.anchorPosition[anchor].z;

stateEstimatorIndex++;
#else
Expand Down

0 comments on commit 6ecbff0

Please sign in to comment.