Commit f61021be authored by Ian McInerney's avatar Ian McInerney

Added outlier rejection to the ranging

parent 3e1e4db4
......@@ -45,6 +45,17 @@
#ifdef COMPUTATION_TYPE_localization
#include "computation_localization.h"
#include "arm_math.h"
#endif
// Outlier rejection
#ifdef COMPUTATION_TYPE_localization
#define RANGING_HISTORY_LENGTH 32
#define OUTLIER_TH 4
static struct {
float32_t history[RANGING_HISTORY_LENGTH];
size_t ptr;
} rangingStats[LOCODECK_NR_OF_ANCHORS];
#endif
// Outlier rejection
......@@ -185,7 +196,19 @@ static uint32_t rxcallback(dwDevice_t *dev) {
options->pressures[current_anchor] = report->asl;
#ifdef COMPUTATION_TYPE_localization
computation_newDistance(current_anchor, options->distance[current_anchor]);
// Outliers rejection
rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
float32_t mean;
float32_t stddev;
arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
float32_t diff = fabsf(mean - options->distance[current_anchor]);
rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor];
if ( diff < (OUTLIER_TH*stddev) ){
computation_newDistance(current_anchor, options->distance[current_anchor]);
}
#endif
#ifdef ESTIMATOR_TYPE_kalman
......@@ -298,7 +321,7 @@ static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
// locSrvSendRangeFloat(current_anchor, options->distance[current_anchor]);
#ifdef COMPUTATION_TYPE_localization
computation_newDistance(current_anchor, options->distance[current_anchor]);
// computation_newDistance(current_anchor, options->distance[current_anchor]);
#endif
succededRanging[current_anchor]++;
}
......@@ -329,6 +352,7 @@ static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
ranging_complete = false;
initiateRanging(dev);
}
return MAX_TIMEOUT;
break;
case eventReceiveTimeout:
......
......@@ -10,6 +10,7 @@ static float ranges[LOCODECK_NR_OF_ANCHORS];
* This is used to get the distance from the ranging deck
*/
void computation_newDistance(int anchor, float newRange) {
DEBUG_PRINT("D: %f\n", newRange);
ranges[anchor] = newRange;
}
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment