Go to the documentation of this file.00001
00002
00003 class Kalman:
00004 def __init__(self, Q=.002, R=1.0, P=.01):
00005 self.x = None
00006 self.Q = Q
00007 self.R = R
00008 self.P = P
00009 self.p = None
00010
00011 def update(self, values):
00012 N = len(values)
00013 if self.x is None:
00014 self.x = values
00015 self.p = [self.P]*N
00016 else:
00017 for i in range(N):
00018 self.p[i] += self.Q
00019 k = self.p[i] / (self.p[i] + self.R)
00020 self.x[i] += k * (values[i] - self.x[i])
00021 self.p[i] = (1 - k) * self.p[i]
00022
00023 def values(self):
00024 return self.x
00025