openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

74 lines
1.9 KiB

5 years ago
import numpy as np
class RunningStat():
# tracks realtime mean and standard deviation without storing any data
def __init__(self, priors=None, max_trackable=-1):
self.max_trackable = max_trackable
if priors is not None:
# initialize from history
self.M = priors[0]
self.S = priors[1]
self.n = priors[2]
self.M_last = self.M
self.S_last = self.S
else:
self.reset()
def reset(self):
self.M = 0.
self.S = 0.
self.M_last = 0.
self.S_last = 0.
self.n = 0
def push_data(self, new_data):
# short term memory hack
if self.max_trackable < 0 or self.n < self.max_trackable:
self.n += 1
if self.n == 0:
self.M_last = new_data
self.M = self.M_last
self.S_last = 0.
else:
self.M = self.M_last + (new_data - self.M_last) / self.n
self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M);
self.M_last = self.M
self.S_last = self.S
def mean(self):
return self.M
def variance(self):
if self.n >= 2:
return self.S / (self.n - 1.)
else:
return 0
def std(self):
return np.sqrt(self.variance())
def params_to_save(self):
return [self.M, self.S, self.n]
class RunningStatFilter():
def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
self.raw_stat = RunningStat(raw_priors, max_trackable)
self.filtered_stat = RunningStat(filtered_priors, max_trackable)
def reset(self):
self.raw_stat.reset()
self.filtered_stat.reset()
def push_and_update(self, new_data):
_std_last = self.raw_stat.std()
self.raw_stat.push_data(new_data)
_delta_std = self.raw_stat.std() - _std_last
if _delta_std<=0:
self.filtered_stat.push_data(new_data)
else:
pass
# self.filtered_stat.push_data(self.filtered_stat.mean())
# class SequentialBayesian():