Beispiel #1
0
def getmeas():
    return np.array((norm(i.getAcc()),norm(i.getHeading()))).reshape((6))
Beispiel #2
0
    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