Ultrasonic Median Library NewPing muss zusätzlich installiert werden. https://www.arduino.cc/reference/en/libraries/newping/

median.ino

#include <NewPing.h>
#define TRIGGER_LEFT 1
#define ECHO_LEFT 2
#define TRIGGER_RIGHT 3
#define ECHO_RIGHT 4
#define SENSOR_MAX_RANGE 400

NewPing left(TRIGGER_LEFT, ECHO_LEFT, SENSOR_MAX_RANGE);
NewPing right(TRIGGER_RIGHT, ECHO_RIGHT, SENSOR_MAX_RANGE);

// buffer for median filter
int lastReadingsRight[5];
int lastReadingsLeft[5];
int nrReadingsRight = 0;
int nrReadingsLeft = 0;
int medianResultRight;
int medianResultLeft;

int distanceL = 0;
int distanceR = 0;

//////////////////////////////////////////////////
void initUltrasonic() {
  //pinmodes for ultrasonic
  pinMode(1, OUTPUT);
  pinMode(2, INPUT);
  pinMode(3, OUTPUT);
  pinMode(4, INPUT);
  for (int i = 0; i < 5; i++) lastReadingsRight[i] = 400;
  for (int i = 0; i < 5; i++) lastReadingsLeft[i] = 400;
}


#define swap(a,b) a ^= b; b ^= a; a ^= b;
#define sort(a,b) if(a>b){ swap(a,b); }
int median(int a, int b, int c, int d, int e)
{
  sort(a, b);
  sort(d, e);
  sort(a, c);
  sort(b, c);
  sort(a, d);
  sort(c, d);
  sort(b, e);
  sort(b, c);
  // this last one is obviously unnecessary for the median
  //sort(d,e);

  return c;
}

int getDistance(char side) {
  int distance;
  if (side == 'l') {
    distance = left.ping_cm();
  }
  if (side == 'r') {
    distance = right.ping_cm();
  }
  if (distance == 0) {
    distance = 400;
  }
  return distance;
}

int getMedianRight() {
  int placeholder_medianR = medianResultRight;
  medianResultRight = median(
                        lastReadingsRight[0],
                        lastReadingsRight[1],
                        lastReadingsRight[2],
                        lastReadingsRight[3],
                        lastReadingsRight[4]);
  if (medianResultRight == 0) {
    // take the last median instead
    medianResultRight = placeholder_medianR;
  }
}
int getMedianLeft() {
  int placeholder_medianL = medianResultLeft;
  medianResultLeft = median(
                       lastReadingsLeft[0],
                       lastReadingsLeft[1],
                       lastReadingsLeft[2],
                       lastReadingsLeft[3],
                       lastReadingsLeft[4]
                     );
  if (medianResultLeft == 0) {
    // take the last median instead
    medianResultLeft = placeholder_medianL;
  }
  return medianResultLeft;
}


void setup() {
  // put your setup code here, to run once:
  initUltrasonic();
  Serial.begin(9600);

}

void loop() {
  // put your main code here, to run repeatedly:
  lastReadingsRight[nrReadingsRight++ % 5] = getDistance('r');
  lastReadingsLeft[nrReadingsLeft++ % 5] =  getDistance('l');

  Serial.println(getMedianLeft());
  Serial.println(getMedianRight());

}