#315 Updated outlier filter

Changed double to float Removed unused estimated position param
bitcraze · May 22, 2018 · a5dd1d5 · a5dd1d5
1 parent a504122
commit a5dd1d5
Show file tree

Hide file tree

Showing 6 changed files with 12 additions and 20 deletions.
diff --git a/src/deck/drivers/interface/outlierFilter.h b/src/deck/drivers/interface/outlierFilter.h
@@ -29,7 +29,7 @@
 
 #include "stabilizer_types.h"
 
-bool outlierFilterValidateTdoa(tdoaMeasurement_t* tdoa, point_t* estimatedPosition);
+bool outlierFilterValidateTdoa(tdoaMeasurement_t* tdoa);
 void outlierFilterReset();
 
 #endif // __OUTLIER_FILTER_H__
diff --git a/src/deck/drivers/src/lpsTdoa2Tag.c b/src/deck/drivers/src/lpsTdoa2Tag.c
@@ -107,9 +107,6 @@ static uint64_t truncateToAnchorTimeStamp(uint64_t fullTimeStamp) {
 }
 
 static void enqueueTDOA(uint8_t anchorA, uint8_t anchorB, double distanceDiff) {
-  point_t estimatedPos;
-  estimatorKalmanGetEstimatedPos(&estimatedPos);
-
   tdoaMeasurement_t tdoa = {
     .stdDev = MEASUREMENT_NOISE_STD,
     .distanceDiff = distanceDiff,
@@ -118,7 +115,7 @@ static void enqueueTDOA(uint8_t anchorA, uint8_t anchorB, double distanceDiff) {
     .anchorPosition[1] = options->anchorPosition[anchorB]
   };
 
-  if (outlierFilterValidateTdoa(&tdoa, &estimatedPos)) {
+  if (outlierFilterValidateTdoa(&tdoa)) {
     if (options->combinedAnchorPositionOk ||
         (options->anchorPosition[anchorA].timestamp && options->anchorPosition[anchorB].timestamp)) {
       stats.packetsToEstimator++;

diff --git a/src/deck/drivers/src/lpsTdoaTagEngine.c b/src/deck/drivers/src/lpsTdoaTagEngine.c
@@ -264,16 +264,13 @@ static uint64_t truncateToAnchorTimeStamp(uint64_t fullTimeStamp) {
 }
 
 static void enqueueTDOA(const anchorInfo_t* anchorACtx, const anchorInfo_t* anchorBCtx, double distanceDiff) {
-  point_t estimatedPos;
-  estimatorKalmanGetEstimatedPos(&estimatedPos);
-
   tdoaMeasurement_t tdoa = {
     .stdDev = MEASUREMENT_NOISE_STD,
     .distanceDiff = distanceDiff
   };
 
   if (getAnchorPosition(anchorACtx, &tdoa.anchorPosition[0]) && getAnchorPosition(anchorBCtx, &tdoa.anchorPosition[1])) {
-    if (outlierFilterValidateTdoa(&tdoa, &estimatedPos)) {
+    if (outlierFilterValidateTdoa(&tdoa)) {
       lpsTdoaStats.packetsToEstimator++;
       estimatorKalmanEnqueueTDOA(&tdoa);
 

diff --git a/src/deck/drivers/src/outlierFilter.c b/src/deck/drivers/src/outlierFilter.c
@@ -28,11 +28,11 @@
 #include "outlierFilter.h"
 
 static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoaMeasurement_t* tdoa);
-static double distanceSq(const point_t* a, const point_t* b);
-static double sq(double a) {return a * a;}
+static float distanceSq(const point_t* a, const point_t* b);
+static float sq(float a) {return a * a;}
 
 
-bool outlierFilterValidateTdoa(tdoaMeasurement_t* tdoa, point_t* estimatedPosition) {
+bool outlierFilterValidateTdoa(tdoaMeasurement_t* tdoa) {
   return isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoa);
 }
 
@@ -41,11 +41,11 @@ void outlierFilterReset() {
 }
 
 static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(tdoaMeasurement_t* tdoa) {  
-  double anchorDistanceSq = distanceSq(&tdoa->anchorPosition[0], &tdoa->anchorPosition[1]);
-  double distanceDiffSq = sq(tdoa->distanceDiff);
+  float anchorDistanceSq = distanceSq(&tdoa->anchorPosition[0], &tdoa->anchorPosition[1]);
+  float distanceDiffSq = sq(tdoa->distanceDiff);
   return (distanceDiffSq < anchorDistanceSq);
 }
 
-static double distanceSq(const point_t* a, const point_t* b) {
+static float distanceSq(const point_t* a, const point_t* b) {
   return sq(a->x - b->x) + sq(a->y - b->y) + sq(a->z - b->z);
 }
diff --git a/test/deck/drivers/src/TestLpsTdoa2Tag.c b/test/deck/drivers/src/TestLpsTdoa2Tag.c
@@ -125,7 +125,6 @@ void setUp(void) {
   dwGetReceiveTimestamp_resetMock();
 
   mockKalmanEstimator_resetMock();
-  estimatorKalmanGetEstimatedPos_Ignore();
 
   options.combinedAnchorPositionOk = true;
 

diff --git a/test/deck/drivers/src/TestOutlierFilter.c b/test/deck/drivers/src/TestOutlierFilter.c
@@ -6,7 +6,6 @@
 #include "mock_cfassert.h"
 
 static tdoaMeasurement_t tdoa;
-static point_t estimatedPosition;
 
 void setUp(void) {
   outlierFilterReset();
@@ -33,7 +32,7 @@ void testThatSamplesAreAcceptedWhenTdoaIsCloserThanDistanceBetweenAnchors() {
   bool expected = true;
 
   // Test
-  bool actual = outlierFilterValidateTdoa(&tdoa, &estimatedPosition);
+  bool actual = outlierFilterValidateTdoa(&tdoa);
 
   // Assert
   TEST_ASSERT_EQUAL(actual, expected);
@@ -46,7 +45,7 @@ void testThatSamplesAreRejectedWhenTdoaIsGreaterThanDistanceBetweenAnchors() {
   bool expected = false;
 
   // Test
-  bool actual = outlierFilterValidateTdoa(&tdoa, &estimatedPosition);
+  bool actual = outlierFilterValidateTdoa(&tdoa);
 
   // Assert
   TEST_ASSERT_EQUAL(actual, expected);
@@ -59,7 +58,7 @@ void testThatSamplesAreRejectedWhenTdoaIsGreaterButNegativeThanDistanceBetweenAn
   bool expected = false;
 
   // Test
-  bool actual = outlierFilterValidateTdoa(&tdoa, &estimatedPosition);
+  bool actual = outlierFilterValidateTdoa(&tdoa);
 
   // Assert
   TEST_ASSERT_EQUAL(actual, expected);