def test1():
kal = KalmanGeneric(1, 2, 1)
dt = 0.1
sigma_theta = 0.01
kal.setMatH([[1,0]])
kal.setMatF([[1, dt],[0, 1]])
kal.setQ([[dt*dt*dt*dt/4., dt*dt*dt/2.], [dt*dt*dt/2., dt*dt]])
kal.setR([[sigma_theta*sigma_theta]])
kal.setB([[dt*dt/2.],[dt]])
theta = 0
omega = 0
#alpha = 0.2
kal.setX0([[0],[0]])
kal.setP0([[1000, 0], [0, 1000]])
av = []
thv = []
ov = []
tkv = []
okv = []
tv = []
for t in range(0, 500):
alpha = random.normalvariate(0, 1) + 10;
omega = omega + alpha*dt
theta = theta + omega*dt
[[t_k], [v_k]] = kal.newMeasure([[random.normalvariate(theta, sigma_theta)]],[[10]])
tv.append(t*dt)
av.append(alpha)
ov.append(omega)
thv.append(theta)
tkv.append(t_k - theta)
okv.append(v_k - omega)
#print "Real value %s / %s" % (theta, omega)
#print " %s / %s" % (t_k, v_k)
#print kal
plt.subplot(3, 1, 1)
plt.plot(tv, tkv, "r", tv, thv, "b")
plt.subplot(3, 1, 2)
plt.plot(tv, okv, "r", tv, ov, "b")
plt.subplot(3, 1, 3)
plt.plot(tv, av, "b")
plt.show()
def test2():
kal = KalmanGeneric(1,2, 0)
dt = 0.1
sigma_omega = 0.01
kal.setMatH([[1,0]])
kal.setMatF([[1, dt],[0, 1]])
kal.setQ([[dt*dt, 0], [0, 1.]])
kal.setR([[sigma_omega*sigma_omega]])
#kal.setB([[0],[0]])
omega = 1
alpha = 0.5
kal.setX0([[1],[0]])
kal.setP0([[1000, 0], [0, 1]])
av = []
ov = []
okv = []
akv = []
tv = []
delta_akv = []
for t in range(0, 500):
alpha = alpha + 0.1
omega = omega + alpha*dt
[o_k, a_k] = kal.newMeasure([[random.normalvariate(omega, sigma_omega)]],[[]])
tv.append(t*dt)
av.append(alpha)
ov.append(omega)
akv.append(a_k[0])
okv.append(o_k[0])
delta_akv.append(alpha-a_k[0])
#print "Real value %s / %s" % (theta, omega)
#print " %s / %s" % (t_k, v_k)
#print kal
plt.subplot(3, 1, 1)
plt.plot(tv, okv, "r", tv, ov, "b")
plt.subplot(3, 1, 2)
plt.plot(tv, akv, "r", tv, av, "b")
plt.subplot(3, 1, 3)
plt.plot(tv, delta_akv, "r")
plt.show()
class PrecisionTest(object):
'''
classdocs
'''
position = None
speed = None
acceleration = None
attitude = None
omega = None
alpha = None
control = None
time = None
old_attitude = None
old_omega = None
old_time = None
v_time = None
true_omega = None
v_omega = None
diff_omega = None
true_alpha = None
v_alpha = None
diff_alpha = None
type = 2
filter = None
queue_alpha = None
size_alpha = 2
def __init__(self, dt):
'''
Constructor
'''
self.v_time = []
self.true_omega = []
self.v_omega =[]
self.diff_omega = []
self.true_alpha = []
self.v_alpha =[]
self.diff_alpha = []
if self.type==1:
self.queue_alpha = []
elif self.type==2:
self.filter = KalmanGeneric(1, 2, 1)
#self.filter.setMatH([[1,0]])
#self.filter.setMatF([[1, dt],[0, 1]])
#self.filter.setQ([[dt*dt, 0], [0, 1.]])
#self.filter.setR([[1.86E-5, 0], [0, 1.87E-5]])
sigma_omega = 1./(131.*4.)
self.filter.setMatH([[1,0]])
self.filter.setMatF([[1, dt],[0, 1]])
self.filter.setQ([[dt*dt*dt*dt/4., dt*dt*dt/2.], [dt*dt*dt/2., dt*dt]])
self.filter.setR([[sigma_omega*sigma_omega]])
self.filter.setB([[dt],[1]])
self.filter.setX0([[0],[0]])
self.filter.setP0([[1, 0], [0, 1]])
def setAngularMeasure(self, quat, omega, t):
self.old_attitude = self.attitude
self.old_omega = self.omega
self.old_time = self.time
self.attitude = quat
self.omega = int(omega[0]*(131*2))/(131.*2.)
#print "%s / %s" % (self.omega, omega[0])
self.time = t
def setControl(self, c):
self.control = c
def computeValues(self):
if self.type==0:
self.computeSingleAverage()
elif self.type==1:
self.computeMultipleAverage()
elif self.type==2:
self.computeKalman()
def computeSingleAverage(self):
if(self.old_time==None):
return
self.alpha = (self.omega - self.old_omega)/(self.time-self.old_time)
def computeMultipleAverage(self):
if(self.old_time==None):
return
alpha = (self.omega - self.old_omega)/(self.time-self.old_time)
if len(self.queue_alpha)>self.size_alpha:
self.queue_alpha.pop()
self.queue_alpha.insert(0, alpha)
self.alpha = 0.
for el in self.queue_alpha:
self.alpha += el
self.alpha /= len(self.queue_alpha)
def computeKalman(self):
if(self.old_time==None):
self.filter.setX0([[self.omega],[0]])
self.filter.setP0([[1000,0], [0,1]])
[[self.omega],[self.alpha]] = self.filter.newMeasure([[self.omega]], [[0]])
#print self.omega, self.alpha
def compare(self, omega, alpha):
if(self.old_time==None):
return
self.v_time.append(self.time)
self.true_omega.append(omega[0])
self.v_omega.append(self.omega)
self.diff_omega.append(self.omega-omega[0])
self.true_alpha.append(alpha[0])
self.v_alpha.append(self.alpha)
self.diff_alpha.append(self.alpha-alpha[0])
def plot(self):
plt.subplot(4, 1, 1)
plt.title("omega")
plt.plot(self.v_time, self.v_omega, "r", self.v_time, self.true_omega, "b")
plt.subplot(4, 1, 2)
plt.title("omega_diff")
plt.plot(self.v_time, self.diff_omega, "r")
plt.subplot(4, 1, 3)
plt.title("alpha")
plt.plot(self.v_time, self.v_alpha, "r", self.v_time, self.true_alpha, "b")
plt.subplot(4, 1, 4)
plt.title("alpha_diff")
plt.plot(self.v_time, self.diff_alpha, "r")
plt.show()