def getmeas():
return np.array((norm(i.getAcc()),norm(i.getHeading()))).reshape((6))
ax,ay,az = frame(q,aref)
bx,by,bz = frame(q,bref)
return np.array((ax,ay,az,bx,by,bz))
#Converts the state q to euler angles
def quat2euler(q):
gx,gy,gz = frame(q,(0.,0.,1.))
a=-np.arctan2(gy,gz)
b=np.arctan2(gx,gz)
bx,by,bz = frame(q,(1,0,0))
c = np.arctan2(by,bx)
return a,b,c
count = 0
lag = 10
aref = norm(i.getAcc())
bref = norm(i.getHeading())
q = np.array((1,0,0,0))
C = computeC(q,q)
Q = Qo(0.01)
R = Ro(.05,1000) # Ea: ErrAcc Eb: ErrMag
P = np.array((
(1,1,1,1),
(1,1,1,1),
(1,1,1,1),
(1,1,1,1),
))
while True:
count = (count +1) % lag